From d4626a4e98e65f0289b719d426d0cfc98ec157ac Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Wed, 24 Jun 2020 20:28:36 +0900 Subject: [PATCH] Add new driver sim - gps_driver_sim --- bringup/config/params.gps_driver.yaml | 17 +++ bringup/launch/driver_sim.launch.py | 1 + bringup/launch/gps_driver_sim.launch.py | 65 +++++++++ bringup/package.xml | 2 + driver_sim/gen_proto_code.sh | 2 +- .../elevator_system/package.xml | 1 - gps_driver_sim/CMakeLists.txt | 50 +++++++ gps_driver_sim/README.md | 3 + .../include/gps_driver_sim/GPSDriverSim.hpp | 49 +++++++ gps_driver_sim/package.xml | 29 ++++ gps_driver_sim/src/GPSDriverSim.cpp | 131 ++++++++++++++++++ gps_driver_sim/src/main.cpp | 27 ++++ 12 files changed, 375 insertions(+), 2 deletions(-) create mode 100644 bringup/config/params.gps_driver.yaml create mode 100644 bringup/launch/gps_driver_sim.launch.py create mode 100644 gps_driver_sim/CMakeLists.txt create mode 100644 gps_driver_sim/README.md create mode 100644 gps_driver_sim/include/gps_driver_sim/GPSDriverSim.hpp create mode 100644 gps_driver_sim/package.xml create mode 100644 gps_driver_sim/src/GPSDriverSim.cpp create mode 100644 gps_driver_sim/src/main.cpp diff --git a/bringup/config/params.gps_driver.yaml b/bringup/config/params.gps_driver.yaml new file mode 100644 index 00000000..958bf31b --- /dev/null +++ b/bringup/config/params.gps_driver.yaml @@ -0,0 +1,17 @@ +gps_driver: + ros__parameters: + use_sim_time: True + + sim: + model: "cloi" + parts: "gps" + manager_ip: "127.0.0.1" + manager_port: 25554 + + remapping_list: ['/tf', '/tf_static'] + + transform: [0.0, 0.0, 0.21, 0.0, 0.0, 0.0] + intensity: False + + frame_id: "gps" + topic_name: "navsatfix" \ No newline at end of file diff --git a/bringup/launch/driver_sim.launch.py b/bringup/launch/driver_sim.launch.py index b8419529..425c5650 100755 --- a/bringup/launch/driver_sim.launch.py +++ b/bringup/launch/driver_sim.launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): 'lidar_driver_sim', 'micom_driver_sim', 'camera_driver_sim', + # 'gps_driver_sim', # 'multi_camera_driver_sim' ] diff --git a/bringup/launch/gps_driver_sim.launch.py b/bringup/launch/gps_driver_sim.launch.py new file mode 100644 index 00000000..c7346d11 --- /dev/null +++ b/bringup/launch/gps_driver_sim.launch.py @@ -0,0 +1,65 @@ +# +# LGE Advanced Robotics Laboratory +# Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea +# All Rights are Reserved. +# +# SPDX-License-Identifier: MIT +# + +import os +import launch.actions +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from simdevice_bringup.common import get_modified_params_with_ns_and_remapping_list +from simdevice_bringup.common import find_robot_name +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.actions import SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + + _robot_name = LaunchConfiguration('robot_name') + + # Get the launch directory + _pkg_name = "simdevice_bringup" + + _config_dir = os.path.join(get_package_share_directory(_pkg_name), 'config') + config_params = os.path.join(_config_dir, 'params.gps_driver.yaml') + + _package_name = 'gps_driver_sim' + _node_name = 'gps_driver' + + # modify config param with namespace + (_config_params, _remapping_list) = get_modified_params_with_ns_and_remapping_list( + config_params, _node_name) + + start_gps_driver_cmd = Node( + package=_package_name, + node_executable=_package_name, + node_name=_node_name, + node_namespace=_robot_name, + remappings=_remapping_list, + parameters=[_config_params], + output='screen') + + declare_launch_argument_rn = DeclareLaunchArgument( + 'robot_name', + default_value=find_robot_name(), + description='It is robot name. same as `node namspace`') + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') + + # Create the launch description and populate + ld = launch.LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + ld.add_action(declare_launch_argument_rn) + + ld.add_action(start_gps_driver_cmd) + + return ld diff --git a/bringup/package.xml b/bringup/package.xml index 51f74b41..828380a1 100644 --- a/bringup/package.xml +++ b/bringup/package.xml @@ -20,6 +20,7 @@ unity_ros lidar_driver_sim micom_driver_sim + gps_driver_sim camera_driver_sim multi_camera_driver_sim elevator_system @@ -30,6 +31,7 @@ unity_ros mcu_driver_sim lidar_driver_sim + gps_driver_sim camera_driver_sim multi_camera_driver_sim elevator_system diff --git a/driver_sim/gen_proto_code.sh b/driver_sim/gen_proto_code.sh index 1a4be2d3..5c6c3975 100755 --- a/driver_sim/gen_proto_code.sh +++ b/driver_sim/gen_proto_code.sh @@ -16,7 +16,7 @@ mkdir -p $TARGET MSG="any param param_v color " MSG+="time vector2d vector3d quaternion pose pose_animation " MSG+="imu image images_stamped image_stamped laserscan laserscan_stamped " -MSG+="micom battery pointcloud" +MSG+="micom battery pointcloud gps " for i in $MSG do diff --git a/elevator_system_sim/elevator_system/package.xml b/elevator_system_sim/elevator_system/package.xml index 15d98922..45d81aec 100644 --- a/elevator_system_sim/elevator_system/package.xml +++ b/elevator_system_sim/elevator_system/package.xml @@ -16,7 +16,6 @@ std_msgs sensor_msgs geometry_msgs - image_transport driver_sim sim_bridge diff --git a/gps_driver_sim/CMakeLists.txt b/gps_driver_sim/CMakeLists.txt new file mode 100644 index 00000000..77a5c155 --- /dev/null +++ b/gps_driver_sim/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################### +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(gps_driver_sim) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -fstack-protector -O2) +endif() + +################################################################################ +# Find colcon packages and libraries for colcon and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sim_bridge REQUIRED) +find_package(driver_sim REQUIRED) + +################################################################################ +# Build +################################################################################ +include_directories(include) + +add_executable(${PROJECT_NAME} + src/main.cpp + src/GPSDriverSim.cpp) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + sensor_msgs + geometry_msgs + sim_bridge + driver_sim) + +################################################################################ +# Install +################################################################################ +install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) + +################################################################################ +# Test +################################################################################ + +ament_package() \ No newline at end of file diff --git a/gps_driver_sim/README.md b/gps_driver_sim/README.md new file mode 100644 index 00000000..7625ebd0 --- /dev/null +++ b/gps_driver_sim/README.md @@ -0,0 +1,3 @@ +# GPS Driver Sim + +Currently noise model is not applied yet. \ No newline at end of file diff --git a/gps_driver_sim/include/gps_driver_sim/GPSDriverSim.hpp b/gps_driver_sim/include/gps_driver_sim/GPSDriverSim.hpp new file mode 100644 index 00000000..d90732fb --- /dev/null +++ b/gps_driver_sim/include/gps_driver_sim/GPSDriverSim.hpp @@ -0,0 +1,49 @@ +/** + * @file GPSDriverSim.hpp + * @date 2020-06-26 + * @author Hyunseok Yang + * @brief + * ROS2 GPS Driver class for simulator + * @remark + * @copyright + * LGE Advanced Robotics Laboratory + * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ + +#ifndef _GPSDRIVERSIM_H_ +#define _GPSDRIVERSIM_H_ + +#include "driver_sim/driver_sim.hpp" +#include +#include "protobuf/gps.pb.h" + +class GPSDriverSim : public DriverSim +{ +public: + GPSDriverSim(); + ~GPSDriverSim(); + +private: + virtual void Initialize(); + virtual void Deinitialize(); + virtual void UpdateData(); + +private: + // key for connection + std::string m_hashKeySub; + + std::string frame_id_; + + // buffer from simulation + gazebo::msgs::GPS m_pbBuf; + + // message for ROS2 communictaion + sensor_msgs::msg::NavSatFix msg_navsat; + + // Laser publisher + rclcpp::Publisher::SharedPtr pubNav; +}; +#endif \ No newline at end of file diff --git a/gps_driver_sim/package.xml b/gps_driver_sim/package.xml new file mode 100644 index 00000000..65f9c9ed --- /dev/null +++ b/gps_driver_sim/package.xml @@ -0,0 +1,29 @@ + + + gps_driver_sim + 1.1.0 + virtual gps driver for simulation + Hyunseok Yang + Hyunseok Yang + MIT + + ament_cmake + + sensor_msgs + geometry_msgs + + sim_bridge + driver_sim + rmw + rclcpp + + launch_ros + sim_bridge + driver_sim + rmw + rclcpp + + + ament_cmake + + diff --git a/gps_driver_sim/src/GPSDriverSim.cpp b/gps_driver_sim/src/GPSDriverSim.cpp new file mode 100644 index 00000000..388cc35e --- /dev/null +++ b/gps_driver_sim/src/GPSDriverSim.cpp @@ -0,0 +1,131 @@ +/** + * @file GPSDriverSim.cpp + * @date 2020-06-26 + * @author Hyunseok Yang + * @brief + * ROS2 GPS Driver class for simulator + * @remark + * @copyright + * LGE Advanced Robotics Laboratory + * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ + +#include "gps_driver_sim/GPSDriverSim.hpp" +#include + +using namespace std; + +GPSDriverSim::GPSDriverSim() + : DriverSim("gps_driver_sim") +{ + Start(); +} + +GPSDriverSim::~GPSDriverSim() +{ + Stop(); +} + +void GPSDriverSim::Initialize() +{ + string part_name_; + string topic_name_; + vector transform_; + + get_parameter_or("sim.parts", part_name_, string("gps")); + + get_parameter_or("topic_name", topic_name_, string("scan")); + get_parameter_or("frame_id", frame_id_, string("gps")); + get_parameter_or("transform", transform_, vector({0, 0, 0, 0, 0, 0})); + + DBG_SIM_INFO("[CONFIG] sim.part: %s", part_name_.c_str()); + DBG_SIM_INFO("[CONFIG] topic_name: %s", topic_name_.c_str()); + DBG_SIM_INFO("[CONFIG] frame_id: %s", frame_id_.c_str()); + + m_hashKeySub = GetRobotName() + part_name_; + DBG_SIM_INFO("hash Key sub: %s", m_hashKeySub.c_str()); + + geometry_msgs::msg::TransformStamped gps_tf; + tf2::Quaternion convertQuternion; + convertQuternion.setRPY(transform_[3], transform_[4], transform_[5]); + convertQuternion = convertQuternion.normalize(); + + gps_tf.header.frame_id = "base_link"; + gps_tf.child_frame_id = frame_id_; + gps_tf.transform.translation.x = transform_[0]; + gps_tf.transform.translation.y = transform_[1]; + gps_tf.transform.translation.z = transform_[2]; + gps_tf.transform.rotation.x = convertQuternion.x(); + gps_tf.transform.rotation.y = convertQuternion.y(); + gps_tf.transform.rotation.z = convertQuternion.z(); + gps_tf.transform.rotation.w = convertQuternion.w(); + + AddStaticTf2(gps_tf); + + // Get frame for message + msg_navsat.header.frame_id = frame_id_; + + // Fill covariances + // TODO: need to applying noise + msg_navsat.position_covariance[0] = 1.0f; + msg_navsat.position_covariance[4] = 1.0f; + msg_navsat.position_covariance[8] = 1.0f; + msg_navsat.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + // Fill gps status + msg_navsat.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + msg_navsat.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + + // ROS2 Publisher + pubNav = this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); + + GetSimBridge()->Connect(SimBridge::Mode::SUB, m_hashKeySub); +} + +void GPSDriverSim::Deinitialize() +{ + GetSimBridge()->Disconnect(); +} + +void GPSDriverSim::UpdateData() +{ + void *pBuffer = nullptr; + int bufferLength = 0; + + while (IsRunThread()) + { + const bool succeeded = GetSimBridge()->Receive(&pBuffer, bufferLength, false); + + if (!succeeded || bufferLength < 0) + { + DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); + + // try reconnect1ion + if (IsRunThread()) + { + GetSimBridge()->Reconnect(SimBridge::Mode::SUB, m_hashKeySub); + } + + continue; + } + + if (!m_pbBuf.ParseFromArray(pBuffer, bufferLength)) + { + DBG_SIM_ERR("Parsing error, size(%d)", bufferLength); + continue; + } + + m_simTime = rclcpp::Time(m_pbBuf.time().sec(), m_pbBuf.time().nsec()); + + // Fill message with latest sensor data + msg_navsat.header.stamp = m_simTime; + msg_navsat.latitude = m_pbBuf.latitude_deg(); + msg_navsat.longitude = m_pbBuf.longitude_deg(); + msg_navsat.altitude = m_pbBuf.altitude(); + + pubNav->publish(msg_navsat); + } +} \ No newline at end of file diff --git a/gps_driver_sim/src/main.cpp b/gps_driver_sim/src/main.cpp new file mode 100644 index 00000000..7a560117 --- /dev/null +++ b/gps_driver_sim/src/main.cpp @@ -0,0 +1,27 @@ +/** + * @file main.cpp + * @date 2020-06-26 + * @author Hyunseok Yang + * @brief + * ROS2 Node that controls gps sensor for simulation. + * @remark + * @copyright + * LGE Advanced Robotics Laboratory + * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ + +#include "gps_driver_sim/GPSDriverSim.hpp" +using namespace std::literals; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::sleep_for(100ms); + rclcpp::shutdown(); + return 0; +}