Merge pull request #53 from lge-ros2/main

Merge 'main' branch into 'foxy'
lge-ros2 · Feb 4, 2022 · 40ee039 · 40ee039
2 parents fcccccf + d4cb255
commit 40ee039
Show file tree

Hide file tree

Showing 33 changed files with 503 additions and 75 deletions.
diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_actor</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>node for actor plugin</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_base</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>CLOiSim-ROS base class for other CLOiSim-ROS</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_bridge_zmq/package.xml b/cloisim_ros_bridge_zmq/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bridge_zmq</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>bridge for cloisim(simulator) connection through ZMQ</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_bridge_zmq/src/bridge.cpp b/cloisim_ros_bridge_zmq/src/bridge.cpp
@@ -328,7 +328,7 @@ bool Bridge::ConnectSubscriber(const uint16_t port, const string hashKey)
   }
 
   const string bridgeAddress = GetAddress(port);
-  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", this, bridgeAddress.c_str(), nHashTag);
+  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), nHashTag);
 
   if (zmq_connect(pSub_, bridgeAddress.c_str()) < 0)
   {
@@ -344,7 +344,7 @@ bool Bridge::ConnectPublisher(const uint16_t port, const string hashKey)
   m_nHashTagTx = GetHashCode(hashKey);
 
   const string bridgeAddress = GetAddress(port);
-  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", this, bridgeAddress.c_str(), m_nHashTagTx);
+  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), m_nHashTagTx);
 
   if (zmq_connect(pPub_, bridgeAddress.c_str()) < 0)
   {
@@ -360,7 +360,7 @@ bool Bridge::ConnectService(const uint16_t port, const string hashKey)
   m_nHashTagTx = GetHashCode(hashKey);
 
   const string bridgeAddress = GetAddress(port);
-  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", this, bridgeAddress.c_str(), m_nHashTagTx);
+  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), m_nHashTagTx);
 
   if (zmq_connect(pRep_, bridgeAddress.c_str()) < 0)
   {
@@ -376,7 +376,7 @@ bool Bridge::ConnectClient(const uint16_t port, const string hashKey)
   m_nHashTagTx = GetHashCode(hashKey);
 
   const string bridgeAddress = GetAddress(port);
-  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", this, bridgeAddress.c_str(), m_nHashTagTx);
+  DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), m_nHashTagTx);
   if (zmq_connect(pReq_, bridgeAddress.c_str()) < 0)
   {
     lastErrMsg = "ConnectClient Err:" + string(zmq_strerror(zmq_errno()));

diff --git a/cloisim_ros_bringup/CMakeLists.txt b/cloisim_ros_bringup/CMakeLists.txt
@@ -25,6 +25,7 @@ find_package(cloisim_ros_depthcamera REQUIRED)
 find_package(cloisim_ros_multicamera REQUIRED)
 find_package(cloisim_ros_realsense REQUIRED)
 find_package(cloisim_ros_gps REQUIRED)
+find_package(cloisim_ros_imu REQUIRED)
 find_package(cloisim_ros_elevator_system REQUIRED)
 find_package(cloisim_ros_ground_truth REQUIRED)
 find_package(cloisim_ros_world REQUIRED)
@@ -43,6 +44,7 @@ set(dependencies
   cloisim_ros_multicamera
   cloisim_ros_realsense
   cloisim_ros_gps
+  cloisim_ros_imu
   cloisim_ros_elevator_system
   cloisim_ros_ground_truth
   cloisim_ros_world

diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bringup</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
@@ -26,6 +26,7 @@
   <depend>cloisim_ros_multicamera</depend>
   <depend>cloisim_ros_realsense</depend>
   <depend>cloisim_ros_gps</depend>
+  <depend>cloisim_ros_imu</depend>
   <depend>cloisim_ros_elevator_system</depend>
   <depend>cloisim_ros_ground_truth</depend>
   <depend>cloisim_ros_world</depend>

diff --git a/cloisim_ros_bringup/src/main.cpp b/cloisim_ros_bringup/src/main.cpp
@@ -18,6 +18,7 @@
 #include <cloisim_ros_depthcamera/depthcamera.hpp>
 #include <cloisim_ros_realsense/realsense.hpp>
 #include <cloisim_ros_gps/gps.hpp>
+#include <cloisim_ros_imu/imu.hpp>
 #include <cloisim_ros_lidar/lidar.hpp>
 #include <cloisim_ros_micom/micom.hpp>
 #include <cloisim_ros_elevator_system/elevator_system.hpp>
@@ -112,6 +113,13 @@ void bringup_target_parts_by_name(const Json::Value item, const string node_type
         else
           node = std::make_shared<cloisim_ros::Gps>(node_options, node_name, model_name);
       }
+      else if (!node_type.compare("IMU"))
+      {
+        if (g_isSingleMode)
+          node = std::make_shared<cloisim_ros::Imu>(node_options, node_name);
+        else
+          node = std::make_shared<cloisim_ros::Imu>(node_options, node_name, model_name);
+      }
     }
     else if (cloisim_ros::BringUpParam::IsWorldSpecificType(node_type))
     {

diff --git a/cloisim_ros_bringup_param/package.xml b/cloisim_ros_bringup_param/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bringup_param</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp
@@ -20,8 +20,9 @@ bool BringUpParam::IsRobotSpecificType(const string node_type)
 {
   return (!node_type.compare("MICOM") || !node_type.compare("JOINTCONTROL") ||
           !node_type.compare("LIDAR") || !node_type.compare("LASER") ||
-          !node_type.compare("CAMERA") || !node_type.compare("DEPTHCAMERA") || !node_type.compare("MULTICAMERA") || !node_type.compare("REALSENSE") ||
-          !node_type.compare("GPS"));
+          !node_type.compare("CAMERA") || !node_type.compare("DEPTHCAMERA") ||
+          !node_type.compare("MULTICAMERA") || !node_type.compare("REALSENSE") ||
+          !node_type.compare("GPS") || !node_type.compare("IMU"));
 }
 
 bool BringUpParam::IsWorldSpecificType(const string node_type)

diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_camera</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>virtual camera for cloisim</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_depthcamera/package.xml b/cloisim_ros_depthcamera/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_depthcamera</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>virtual depth camera for simulator</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_elevator_system/package.xml b/cloisim_ros_elevator_system/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_elevator_system</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>elevator system for simulation</description>
 
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>

diff --git a/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp b/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp
@@ -43,7 +43,7 @@ namespace cloisim_ros
     // message for ROS2 communictaion
     sensor_msgs::msg::NavSatFix msg_nav_;
 
-    // Laser publisher
+    // publisher
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr pub_;
   };
 }

diff --git a/cloisim_ros_gps/package.xml b/cloisim_ros_gps/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_gps</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>virtual gps for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_ground_truth/package.xml b/cloisim_ros_ground_truth/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_ground_truth</name>
-  <version>2.11.7</version>
+  <version>2.12.1</version>
   <description>world plugin to retrieve ground truth</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_imu/CMakeLists.txt b/cloisim_ros_imu/CMakeLists.txt
@@ -0,0 +1,77 @@
+###############################################################################
+# Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 3.5)
+project(cloisim_ros_imu)
+
+include("../cloisim_ros_base/cmake/cloisim_ros_package.cmake")
+cloisim_ros_package()
+
+################################################################################
+# Find colcon packages and libraries for colcon and system dependencies
+################################################################################
+find_package(ament_cmake REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cloisim_ros_base REQUIRED)
+find_package(cloisim_ros_bringup_param REQUIRED)
+
+set(dependencies
+  sensor_msgs
+  cloisim_ros_base
+  cloisim_ros_bringup_param
+)
+
+################################################################################
+# Build
+################################################################################
+add_library(
+  ${PROJECT_NAME}_core SHARED
+  src/imu.cpp
+)
+
+target_include_directories(
+  ${PROJECT_NAME}_core PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}_core
+  ${dependencies}
+)
+
+add_executable(
+  ${STANDALONE_EXEC_NAME}
+  src/main.cpp
+)
+
+target_link_libraries(
+  ${STANDALONE_EXEC_NAME} PRIVATE
+  ${PROJECT_NAME}_core
+)
+
+################################################################################
+# Install
+################################################################################
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS ${PROJECT_NAME}_core
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+install(
+  TARGETS ${STANDALONE_EXEC_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${dependencies})
+
+ament_package()
diff --git a/cloisim_ros_imu/README.md b/cloisim_ros_imu/README.md
@@ -0,0 +1,21 @@
+# CLOiSim-ROS IMU
+
+support ros remapping, --ros-args -r /test:=test
+
+Currently noise model is not applied yet.
+
+```shell
+ros2 run cloisim_ros_imu standalone
+```
+
+or
+
+```shell
+ros2 run cloisim_ros_imu standalone --ros-args -p single_mode:=True -p target_model:=cloi1 -p target_parts_name:=gps
+```
+
+or
+
+```shell
+ros2 run cloisim_ros_imu standalone --ros-args -p target_model:=cloi1 -p target_parts_name:=gps
+```
diff --git a/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp b/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp
@@ -0,0 +1,50 @@
+/**
+ *  @file   gps.hpp
+ *  @date   2021-01-14
+ *  @author Hyunseok Yang
+ *  @brief
+ *        ROS2 GPS 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 _CLOISIM_ROS_IMU_HPP_
+#define _CLOISIM_ROS_IMU_HPP_
+
+#include <cloisim_ros_base/base.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <cloisim_msgs/imu.pb.h>
+
+namespace cloisim_ros
+{
+  class Imu : public Base
+  {
+  public:
+    explicit Imu(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = "");
+    explicit Imu(const std::string namespace_ = "");
+    ~Imu();
+
+  private:
+    void Initialize() override;
+    void Deinitialize() override { };
+
+  private:
+    void PublishData(const std::string &buffer);
+
+  private:
+    // buffer from simulation
+    cloisim::msgs::IMU pb_buf_;
+
+    // IMU msgs
+    sensor_msgs::msg::Imu msg_imu_;
+
+    // publisher
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_;
+  };
+}
+#endif
diff --git a/cloisim_ros_imu/package.xml b/cloisim_ros_imu/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cloisim_ros_imu</name>
+  <version>2.12.1</version>
+  <description>virtual gps 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>cloisim_ros_bringup_param</depend>
+  <depend>cloisim_ros_base</depend>
+  <depend>sensor_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>