Merge pull request #66 from lge-ros2/main

Merge into humble
lge-ros2 · Dec 12, 2023 · d4f8382 · d4f8382
2 parents b79d075 + 7a95b7b
commit d4f8382
Show file tree

Hide file tree

Showing 25 changed files with 40 additions and 25 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.4.1</version>
+  <version>3.4.2</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>3.4.1</version>
+  <version>3.4.2</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
@@ -168,7 +168,10 @@ void Base::AddPublisherThread(
           int bufferLength = 0;
           const bool succeeded = GetBufferFromSimulator(bridge_ptr, &buffer_ptr, bufferLength);
           if (!succeeded || bufferLength < 0)
+          {
+            DBG_ERR("[%s] Failed to get buffer(%d) <= Sim, %s", get_name(), bufferLength, zmq_strerror(zmq_errno()));
             continue;
+          }
 
           if (IsRunThread() == false)
             break;

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
@@ -28,4 +28,13 @@
 #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__)
 
+#define __SIM_LOG_WITHOUT_FUNC(COLOR_CODE, STR_FORMAT, ...) \
+  printf(COLOR_CODE STR_FORMAT RESET "\n", ##__VA_ARGS__)
+//   printf(COLOR_CODE "[%s][%d] " STR_FORMAT RESET "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__)
+
+#define DBG_INFO(STR_FORMAT, ...) __SIM_LOG_WITHOUT_FUNC(CYAN, STR_FORMAT, ##__VA_ARGS__)
+#define DBG_WRN(STR_FORMAT, ...) __SIM_LOG_WITHOUT_FUNC(YELLOW, STR_FORMAT, ##__VA_ARGS__)
+#define DBG_MSG(STR_FORMAT, ...) __SIM_LOG_WITHOUT_FUNC(GREEN, STR_FORMAT, ##__VA_ARGS__)
+#define DBG_ERR(STR_FORMAT, ...) __SIM_LOG_WITHOUT_FUNC(BOLDRED, STR_FORMAT, ##__VA_ARGS__)
+
 #endif  // CLOISIM_ROS_BRIDGE_ZMQ__LOG_H_
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.4.1</version>
+  <version>3.4.2</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
@@ -412,7 +412,7 @@ bool Bridge::Receive(void **buffer, int &bufferLength, bool isNonBlockingMode)
 
   if ((bufferLength = zmq_msg_recv(&m_msgRx, pSockRx_, (isNonBlockingMode) ? ZMQ_DONTWAIT : 0)) < 0)
   {
-    DBG_SIM_ERR("Failed to receive message len(%d): %s", bufferLength, zmq_strerror(zmq_errno()));
+    // DBG_SIM_ERR("Failed to receive message len(%d): %s", bufferLength, zmq_strerror(zmq_errno()));
     return false;
   }
 

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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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
@@ -233,9 +233,12 @@ void Micom::UpdateImu()
 
 void Micom::UpdateBattery()
 {
-  msg_battery_.header.stamp = GetTime();
-  msg_battery_.voltage = 0.0;
-  msg_battery_.current = 0.0;
+  if (pb_micom_.has_battery())
+  {
+    msg_battery_.header.stamp = GetTime();
+    msg_battery_.voltage = pb_micom_.battery().voltage();
+    msg_battery_.current = 0.0;
+  }
 }
 
 }  // namespace cloisim_ros
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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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.4.1</version>
+  <version>3.4.2</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>