Merge pull request #62 from lge-ros2/main

Merge into humble
lge-ros2 · Aug 8, 2023 · 88bf172 · 88bf172
2 parents f68ef96 + 637590b
commit 88bf172
Show file tree

Hide file tree

Showing 27 changed files with 60 additions and 65 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>3.3.0</version>
+  <version>3.3.1</version>
   <description>node for actor plugin</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_base/include/cloisim_ros_base/base.hpp b/cloisim_ros_base/include/cloisim_ros_base/base.hpp
@@ -46,7 +46,6 @@ class Base : public rclcpp::Node
  protected:
   void SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const std::string child_frame_id, const std::string header_frame_id = "base_link");
   void SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const cloisim::msgs::Pose transform, const std::string child_frame_id, const std::string header_frame_id = "base_link");
-  void SetStaticTf2(const std::string child_frame_id, const std::string header_frame_id);
   void SetStaticTf2(const cloisim::msgs::Pose transform, const std::string parent_header_frame_id = "base_link");
 
   void Start(const bool enable_tf_publish = true);

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>3.3.0</version>
+  <version>3.3.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_base/src/base.cpp b/cloisim_ros_base/src/base.cpp
@@ -283,13 +283,6 @@ void Base::SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const msgs::
   SetQuaternionMessageToGeometry(transform.orientation(), target_msg.transform.rotation);
 }
 
-void Base::SetStaticTf2(const string child_frame_id, const string header_frame_id)
-{
-  auto emptyPose = IdentityPose();
-  emptyPose.set_name(child_frame_id);
-  SetStaticTf2(emptyPose, header_frame_id);
-}
-
 void Base::SetStaticTf2(const msgs::Pose transform, const string parent_header_frame_id)
 {
   geometry_msgs::msg::TransformStamped static_tf;

diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp
@@ -16,7 +16,7 @@
 #ifndef _CLOISIM_ROS_BRIDGE_ZMQ_HPP_
 #define _CLOISIM_ROS_BRIDGE_ZMQ_HPP_
 
-#include "debug_log.h"
+#include "log.h"
 #include <string>
 #include <zmq.h>
 
@@ -76,8 +76,8 @@ class Bridge
   zmq_msg_t m_msgRx;         // for subscriber and reply
   std::size_t m_nHashTagTx;  // for publisher and request
 
-  void* pSockTx_;  // for Send function
-  void* pSockRx_;  // for Recieve function
+  void* pSockTx_;            // for Send function
+  void* pSockRx_;            // for Recieve function
 
   std::string lastErrMsg;
 

diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/debug_log.h b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/debug_log.h
diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h
@@ -0,0 +1,31 @@
+/**
+ *  @file   log.h
+ *  @date   2020-04-21
+ *  @author Hyunseok Yang
+ *  @brief
+ *      the following are UBUNTU/LINUX, and MacOS ONLY terminal color codes.
+ *  @remark
+ *  @copyright
+ *      LGE Advanced Robotics Laboratory
+ *      Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea
+ *      All Rights are Reserved.
+ *
+ *      SPDX-License-Identifier: MIT
+ */
+#ifndef _LOG_H_
+#define _LOG_H_
+
+#include "term_color.h"
+
+#pragma GCC system_header
+
+#define __SIM_LOG(COLOR_CODE, STR_FORMAT, ...) \
+  printf(COLOR_CODE "[%s] " STR_FORMAT RESET "\n", __FUNCTION__, ##__VA_ARGS__)
+//   printf(COLOR_CODE "[%s][%d] " STR_FORMAT RESET "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__)
+
+#define DBG_SIM_INFO(STR_FORMAT, ...) __SIM_LOG(CYAN, STR_FORMAT, ##__VA_ARGS__)
+#define DBG_SIM_WRN(STR_FORMAT, ...) __SIM_LOG(YELLOW, STR_FORMAT, ##__VA_ARGS__)
+#define DBG_SIM_MSG(STR_FORMAT, ...) __SIM_LOG(GREEN, STR_FORMAT, ##__VA_ARGS__)
+#define DBG_SIM_ERR(STR_FORMAT, ...) __SIM_LOG(BOLDRED, STR_FORMAT, ##__VA_ARGS__)
+
+#endif
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>3.3.0</version>
+  <version>3.3.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_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>3.3.0</version>
+  <version>3.3.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/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>3.3.0</version>
+  <version>3.3.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_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>3.3.0</version>
+  <version>3.3.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>3.3.0</version>
+  <version>3.3.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>3.3.0</version>
+  <version>3.3.1</version>
   <description>elevator system for simulation</description>
 
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>

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>3.3.0</version>
+  <version>3.3.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>3.3.0</version>
+  <version>3.3.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/package.xml b/cloisim_ros_imu/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_imu</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>virtual imu 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_joint_control/package.xml b/cloisim_ros_joint_control/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_joint_control</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>joint_control package for simulator</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_lidar/package.xml b/cloisim_ros_lidar/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_lidar</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>virtual lidar 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_micom/package.xml b/cloisim_ros_micom/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_micom</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>micom package for simulator</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp
@@ -55,13 +55,16 @@ void Micom::Initialize()
   const auto hashKeyTf = GetTargetHashKey("Tf");
   DBG_SIM_INFO("hashKey: info(%s) pub(%s) sub(%s) tf(%s)", hashKeyInfo.c_str(), hashKeyPub.c_str(), hashKeySub.c_str(), hashKeyTf.c_str());
 
-  SetStaticTf2("base_link", "base_footprint");
+  {
+    auto base_link_pose = IdentityPose();
+    base_link_pose.set_name("base_link");
+    SetStaticTf2(base_link_pose, "base_footprint");
+  }
 
   info_bridge_ptr = CreateBridge();
   if (info_bridge_ptr != nullptr)
   {
     info_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo);
-
     GetStaticTransforms(info_bridge_ptr);
   }
 
@@ -130,7 +133,6 @@ string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg)
   angular_ptr->set_z(msg->angular.z);
 
   // DBG_SIM_INFO("%f %f", linear_ptr->x(), angular_ptr->z());
-
   // m/s velocity input
   // double vel_left_wheel = (vel_lin - (vel_rot * (0.50f) / 2.0));
   // double vel_right_wheel = (vel_lin + (vel_rot * (0.50f) / 2.0));

diff --git a/cloisim_ros_msgs/package.xml b/cloisim_ros_msgs/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_msgs</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>interfaces package for cloisim_ros</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
 

diff --git a/cloisim_ros_multicamera/package.xml b/cloisim_ros_multicamera/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_multicamera</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>virtual multi-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_protobuf_msgs/package.xml b/cloisim_ros_protobuf_msgs/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_protobuf_msgs</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>CLOiSim-ROS interafces for communication between simulator and 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_realsense/package.xml b/cloisim_ros_realsense/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_realsense</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>virtual realsense 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_sonar/package.xml b/cloisim_ros_sonar/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_sonar</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>virtual sonar 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_websocket_service/package.xml b/cloisim_ros_websocket_service/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_websocket_service</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>websocket service for cloisim(simulator) connection port control</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

diff --git a/cloisim_ros_world/package.xml b/cloisim_ros_world/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_world</name>
-  <version>3.3.0</version>
+  <version>3.3.1</version>
   <description>Utilities to interface with Unity through ROS.</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>