Add new driver sim

- gps_driver_sim

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"

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'
     ]
 

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

bringup/package.xml

@@ -20,6 +20,7 @@
   <build_depend>unity_ros</build_depend>
   <build_depend>lidar_driver_sim</build_depend>
   <build_depend>micom_driver_sim</build_depend>
+  <build_depend>gps_driver_sim</build_depend>
   <build_depend>camera_driver_sim</build_depend>
   <build_depend>multi_camera_driver_sim</build_depend>
   <build_depend>elevator_system</build_depend>
@@ -30,6 +31,7 @@
   <exec_depend>unity_ros</exec_depend>
   <exec_depend>mcu_driver_sim</exec_depend>
   <exec_depend>lidar_driver_sim</exec_depend>
+  <exec_depend>gps_driver_sim</exec_depend>
   <exec_depend>camera_driver_sim</exec_depend>
   <exec_depend>multi_camera_driver_sim</exec_depend>
   <exec_depend>elevator_system</exec_depend>

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

elevator_system_sim/elevator_system/package.xml

@@ -16,7 +16,6 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>image_transport</depend>
 
   <build_depend>driver_sim</build_depend>
   <build_depend>sim_bridge</build_depend>

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()

gps_driver_sim/README.md

@@ -0,0 +1,3 @@
+# GPS Driver Sim
+
+Currently noise model is not applied yet.

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 <sensor_msgs/msg/nav_sat_fix.hpp>
+#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<sensor_msgs::msg::NavSatFix>::SharedPtr pubNav;
+};
+#endif

gps_driver_sim/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>gps_driver_sim</name>
+  <version>1.1.0</version>
+  <description>virtual gps driver for simulation</description>
+  <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
+  <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <build_depend>sim_bridge</build_depend>
+  <build_depend>driver_sim</build_depend>
+  <build_depend>rmw</build_depend>
+  <build_depend>rclcpp</build_depend>
+
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>sim_bridge</exec_depend>
+  <exec_depend>driver_sim</exec_depend>
+  <exec_depend>rmw</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <tf2/LinearMath/Quaternion.h>
+
+using namespace std;
+
+GPSDriverSim::GPSDriverSim()
+    : DriverSim("gps_driver_sim")
+{
+  Start();
+}
+
+GPSDriverSim::~GPSDriverSim()
+{
+  Stop();
+}
+
+void GPSDriverSim::Initialize()
+{
+  string part_name_;
+  string topic_name_;
+  vector<double> 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<double>({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<sensor_msgs::msg::NavSatFix>(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);
+  }
+}