Merge pull request #59 from lge-ros2/main

Merge into humble

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.2.0</version>
+  <version>3.2.1</version>
   <description>node for actor plugin</description>
 
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_base/include/cloisim_ros_base/camera_helper.h

@@ -163,15 +163,11 @@ static cloisim::msgs::CameraSensor GetCameraSensorMessage(
 
   cloisim::msgs::CameraSensor cameraSensorInfo;
   if (reply.size() <= 0)
-  {
     DBG_SIM_ERR("Failed to get camera info, length(%ld)", reply.size());
-  }
   else
   {
     if (cameraSensorInfo.ParseFromString(reply) == false)
-    {
       DBG_SIM_ERR("Failed to Parsing Proto buffer buffer_ptr(%p) length(%ld)", reply.data(), reply.size());
-    }
   }
 
   return cameraSensorInfo;

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.2.0</version>
+  <version>3.2.1</version>
   <description>CLOiSim-ROS base class for other CLOiSim-ROS</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

cloisim_ros_base/src/base.cpp

@@ -230,8 +230,9 @@ void Base::GetRos2Parameter(zmq::Bridge* const bridge_ptr)
     return;
 
   const auto reply = RequestReplyMessage(bridge_ptr, "request_ros2");
-
   if (reply.ByteSizeLong() <= 0)
+    DBG_SIM_ERR("Failed to get ROS2 common info, length(%ld)", reply.ByteSizeLong());
+  else
   {
     if (reply.IsInitialized() && reply.name() == "ros2")
     {
@@ -253,8 +254,6 @@ void Base::GetRos2Parameter(zmq::Bridge* const bridge_ptr)
         }
       }
     }
-    else
-      DBG_SIM_ERR("Failed to get ROS2 common info, length(%ld)", reply.ByteSizeLong());
   }
 }
 

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.2.0</version>
+  <version>3.2.1</version>
   <description>bridge for cloisim(simulator) connection through ZMQ</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
 
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_bringup/src/main.cpp

@@ -31,7 +31,7 @@ void make_bringup_list(const Json::Value result_map, const string target_model,
 
 void remove_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger& logger)
 {
-  ERR(logger, "Remove all nodes=" << g_node_map_list.size());
+  ERR(logger, "Remove all nodes(" << g_node_map_list.size() << ")");
   for (auto it = g_node_map_list.cbegin(); it != g_node_map_list.cend(); it++)
   {
     const auto& key = it->first;
@@ -40,7 +40,7 @@ void remove_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger&
     const auto node = get<2>(value);
     executor.remove_node(node);
     const auto node_info = get<0>(key) + "/" + get<1>(key) + "/" + get<2>(key);
-    WARN(logger, " > Node removed=" << node_info);
+    WARN(logger, "> Node(" << node_info << ") removed");
   }
 
   g_node_map_list.clear();

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.2.0</version>
+  <version>3.2.1</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
 
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual camera for cloisim</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual depth camera for simulator</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

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.2.0</version>
+  <version>3.2.1</version>
   <description>elevator system for simulation</description>
 
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual gps for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>world plugin to retrieve ground truth</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual gps for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>joint_control package for simulator</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual lidar for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>micom package for simulator</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>interfaces package for cloisim_ros</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
 

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual multi-camera for simulator</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_multicamera/src/multicamera.cpp

@@ -67,7 +67,7 @@ void MultiCamera::Initialize()
 
       // Image publisher
       const auto topic_base_name_ = GetPartsName() + "/" + frame_id;
-      DBG_SIM_INFO("topic_base_name:%s", topic_base_name_.c_str());
+      // DBG_SIM_INFO("topic_base_name: %s", topic_base_name_.c_str());
 
       sensor_msgs::msg::Image msg_img;
       msg_img.header.frame_id = frame_id;
@@ -88,14 +88,16 @@ void MultiCamera::Initialize()
 
 void MultiCamera::Deinitialize()
 {
-  for (auto pub_ : pubs_)
-  {
+  for (auto &pub_ : pubs_)
     pub_.shutdown();
-  }
+  pubs_.clear();
 }
 
 void MultiCamera::PublishData(const string &buffer)
 {
+  if (pubs_.size() == 0)
+    return;
+
   if (!pb_buf_.ParseFromString(buffer))
   {
     DBG_SIM_ERR("Parsing error, size(%d)", buffer.length());
@@ -104,6 +106,13 @@ void MultiCamera::PublishData(const string &buffer)
 
   SetTime(pb_buf_.time());
 
+  if (int(camera_info_manager_.size()) != pb_buf_.image_size())
+  {
+    DBG_SIM_ERR("camera_info_manager is not ready for multi-camera %d != %d",
+                camera_info_manager_.size(), pb_buf_.image_size());
+    return;
+  }
+
   for (auto i = 0; i < pb_buf_.image_size(); i++)
   {
     auto img = &pb_buf_.image(i);
@@ -120,6 +129,7 @@ void MultiCamera::PublishData(const string &buffer)
     // Publish camera info
     auto camera_info_msg = camera_info_manager_[i]->getCameraInfo();
     camera_info_msg.header.stamp = GetTime();
+
     pubs_.at(i).publish(*msg_img, camera_info_msg);
   }
 }

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.2.0</version>
+  <version>3.2.1</version>
   <description>CLOiSim-ROS interafces for communication between simulator and CLOiSim-ROS</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>virtual realsense for simulator</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_realsense/src/realsense.cpp

@@ -162,10 +162,9 @@ void RealSense::InitializeImu(zmq::Bridge* const info_ptr, zmq::Bridge* const da
 
 void RealSense::Deinitialize()
 {
-  for (auto pub : pubs_)
-  {
+  for (auto& pub : pubs_)
     pub.second.shutdown();
-  }
+  pubs_.clear();
 }
 
 void RealSense::GetActivatedModules(zmq::Bridge* const bridge_ptr)

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.2.0</version>
+  <version>3.2.1</version>
   <description>websocket service for cloisim(simulator) connection port control</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>

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.2.0</version>
+  <version>3.2.1</version>
   <description>Utilities to interface with Unity through ROS.</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <author email="[email protected]">Hyunseok Yang</author>