Merge into jazzy

.vscode/c_cpp_properties.json

@@ -5,10 +5,10 @@
             "includePath": [
                 "/opt/ros/humble/include",
                 "${workspaceFolder}/**",
-                "${workspaceFolder}/../../install/perception_msgs/include/",
-                "${workspaceFolder}/../../install/elevator_system_msgs/include/",
-                "${workspaceFolder}/../../install/cloisim_ros_protobuf_msgs/include/",
-                "${workspaceFolder}/../../install/cloisim_ros_msgs/include/",
+                "${workspaceFolder}/../../../install/perception_msgs/include/",
+                "${workspaceFolder}/../../../install/elevator_system_msgs/include/",
+                "${workspaceFolder}/../../../install/cloisim_ros_protobuf_msgs/include/",
+                "${workspaceFolder}/../../../install/cloisim_ros_msgs/include/",
                 "${workspaceFolder}/../../../install/perception_msgs/include/",
                 "${workspaceFolder}/../../../install/elevator_system_msgs/include/",
                 "${workspaceFolder}/../../../install/cloisim_ros_protobuf_msgs/include/",
@@ -17,7 +17,7 @@
             "defines": [],
             "compilerPath": "/usr/bin/gcc",
             "cStandard": "gnu17",
-            "cppStandard": "gnu++14",
+            "cppStandard": "gnu++17",
             "intelliSenseMode": "gcc-x64"
         }
     ],

Dockerfile

@@ -1,4 +1,4 @@
-FROM ros:humble-ros-base
+FROM ros:jazzy-ros-base
 
 ENV HOSTNAME cloisim_ros
 

README.md

@@ -1,4 +1,4 @@
-# cloisim_ros (humble version)
+# cloisim_ros (jazzy version)
 
 ROS2 simulation device packages to connect CLOiSim(the unity3D based multi-robot simulator).
 
@@ -7,11 +7,11 @@ ROS2 simulation device packages to connect CLOiSim(the unity3D based multi-robot
 - Latest version: [link](https://github.com/lge-ros2/cloisim/releases/latest)
 - All Releases: [link](https://github.com/lge-ros2/cloisim/releases)
 
-## Install ROS2 humble
+## Install ROS2 jazzy
 
   follow the guideline on below link.
 
-  <https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html>
+  <https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debians.html>
 
 ## Prerequisite
 
@@ -63,29 +63,24 @@ Apply namespaceas each robot as a multi robot mode.
 **Strongly recommend to use this method.**
 
 ```shell
-ros2 launch cloisim_ros_bringup bringup.launch.py
-```
-
-or
-
-```shell
-ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=False
+ros2 launch cloisim_ros_bringup bringup_launch.py
+ros2 launch cloisim_ros_bringup bringup_launch.py single_mode:=False
 ```
 
 #### Turn on single Mode
 
 It shall NOT be applied namespace for robot and the number of robot must BE single in world environment.
 
 ```shell
-ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True
+ros2 launch cloisim_ros_bringup bringup_launch.py single_mode:=True
 ```
 
 ## Running CLOiSim
 
 It provides a script to run CLOiSim easily.
 
 ```shell
-ros2 launch cloisim_ros_bringup cloisim.launch.py sim_path:=/opt/CLOiSim/CLOiSim-2.2.0 world:=lg_seocho.world
+ros2 launch cloisim_ros_bringup cloisim_launch.py sim_path:=/opt/CLOiSim/CLOiSim-2.2.0 world:=lg_seocho.world
 ```
 
 ## Using Docker

cloisim_ros_actor/package.xml

@@ -2,11 +2,9 @@
 <?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>4.0.0</version>
+  <version>4.1.0</version>
   <description>node for actor plugin</description>
-
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
-
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

cloisim_ros_base/README.md

@@ -31,3 +31,13 @@ namespace cloisim_ros
   }
   }
 ```
+
+## parameter
+
+TF/TFStatic is enabled to publish by default.
+
+If you want disable publishing TF and TF Static, just set the parameter `enable_tf:=False`.
+
+Some of cloisim_ros packages are not allowed to disable.
+
+- Actor, Elevator_System, GroundTruth, World

cloisim_ros_base/include/cloisim_ros_base/base.hpp

@@ -29,6 +29,13 @@
 #include <cloisim_ros_bridge_zmq/bridge.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#define INFO_ONCE RCLCPP_INFO_STREAM_ONCE
+#define WARN_ONCE RCLCPP_WARN_STREAM_ONCE
+#define ERR_ONCE RCLCPP_ERROR_STREAM_ONCE
+#define INFO RCLCPP_INFO_STREAM
+#define WARN RCLCPP_WARN_STREAM
+#define ERR RCLCPP_ERROR_STREAM
+
 namespace cloisim_ros
 {
 
@@ -69,7 +76,8 @@ class Base : public rclcpp::Node
   void SetStaticTf2(const cloisim::msgs::Pose transform,
                     const std::string parent_header_frame_id = "base_link");
 
-  void Start(const bool enable_tf_publish = true);
+  void Start();
+  void Start(const bool enable_tf_publish);
   void Stop();
 
   void AddTf2(const geometry_msgs::msg::TransformStamped tf);
@@ -153,8 +161,15 @@ class Base : public rclcpp::Node
   // for ros2 default parameters
   std::string topic_name_;
   std::vector<std::string> frame_id_list_;
+
+  bool enable_tf_publish_;
 };
 
+inline void Base::Start()
+{
+  Start(enable_tf_publish_);
+}
+
 inline void Base::AddTf2(const geometry_msgs::msg::TransformStamped tf)
 {
   m_tf_list.push_back(tf);
@@ -167,7 +182,10 @@ inline void Base::AddStaticTf2(const geometry_msgs::msg::TransformStamped tf)
 
 inline void Base::PublishTF(const geometry_msgs::msg::TransformStamped& tf)
 {
-  m_tf_broadcaster->sendTransform(tf);
+  if (m_tf_broadcaster != nullptr)
+  {
+    m_tf_broadcaster->sendTransform(tf);
+  }
 }
 
 inline cloisim::msgs::Pose Base::GetObjectTransform(zmq::Bridge* const bridge_ptr,

cloisim_ros_base/package.xml

@@ -2,12 +2,13 @@
 <?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>4.0.0</version>
+  <version>4.1.0</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>
   <license>MIT</license>
 
+  <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>

cloisim_ros_base/src/base.cpp

@@ -52,7 +52,9 @@ Base::Base(const string node_name, const string namespace_, const rclcpp::NodeOp
     , m_node_handle(shared_ptr<rclcpp::Node>(this, [](auto) {}))
     , m_static_tf_broadcaster(nullptr)
     , m_tf_broadcaster(nullptr)
+    , enable_tf_publish_(true)
 {
+  get_parameter_or("enable_tf", enable_tf_publish_, bool(true));
 }
 
 Base::~Base()
@@ -74,6 +76,8 @@ void Base::Start(const bool enable_tf_publish)
 
   Initialize();
 
+  DBG_SIM_MSG("node(%s) enable_tf(%d)", get_name(), enable_tf_publish_);
+
   auto callback_static_tf_pub = [this]() -> void
   {
     PublishStaticTF();

cloisim_ros_bridge_zmq/package.xml

@@ -2,11 +2,11 @@
 <?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>4.0.0</version>
+  <version>4.1.0</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>
   <license>MIT</license>
+  <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

cloisim_ros_bridge_zmq/src/bridge.cpp

@@ -50,6 +50,7 @@ Bridge::~Bridge()
 {
   if (pCtx_)
   {
+    zmq_ctx_shutdown(pCtx_);
     zmq_ctx_term(pCtx_);
     pCtx_ = nullptr;
   }
@@ -396,6 +397,8 @@ bool Bridge::CloseSocket(void *&target)
     return false;
   }
 
+  DBG_SIM_WRN("CLOSE ZMQQ!!!!!!!!!!!!!!!!!!!!!!!!");
+
   zmq_close(target);
   target = nullptr;
 

cloisim_ros_bringup/README.md

@@ -21,12 +21,14 @@ ros2 run cloisim_ros_bringup bringup
 
 Apply namespaceas each robot as a multi robot mode
 
+#### ros2 run
+
 ```shell
 ros2 run cloisim_ros_bringup bringup --ros-args -p single_mode:=False
 ros2 run cloisim_ros_bringup bringup
 ```
 
-or
+#### ros2 launch
 
 ```shell
 ros2 launch cloisim_ros_bringup bringup.launch.py
@@ -52,18 +54,22 @@ will NOT apply namespace for robot and the number of robot must BE single in wor
 
 ```shell
 ros2 run cloisim_ros_bringup bringup --ros-args -p single_mode:=True
+ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True
 ```
 
-or
+#### Specific target model without namespace
+
+Specify target model from simulation
 
 ```shell
-ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True
+ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True target_model:=cloi0
 ```
 
-or 
+### Enable or disable TF/TF_Static publishing
 
-specific target model without namespace
+Currently only support "micom" type in cloisim_ros_bringup.
 
 ```shell
-ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True target_model:=cloi0
+ros2 launch cloisim_ros_bringup bringup.launch.py micom.enable_tf:=False
+ros2 run cloisim_ros_bringup bringup --ros-args -p single_mode:=True -p micom.enable_tf:=False
 ```

cloisim_ros_bringup/launch/bringup_launch.py

@@ -1,16 +1,14 @@
-#
-# LGE Advanced Robotics Laboratory
-# Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea
-# All Rights are Reserved.
-#
-# SPDX-License-Identifier: MIT
-#
-
-import os
+"""
+LGE Advanced Robotics Laboratory
+Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea
+All Rights are Reserved.
+
+SPDX-License-Identifier: MIT
+"""
+
 import launch.actions
 from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument
-from launch.actions import SetEnvironmentVariable
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration
 from launch_ros.parameter_descriptions import ParameterValue
 
@@ -21,27 +19,13 @@ def generate_launch_description():
     _target_model = LaunchConfiguration('target_model')
     _target_parts_type = LaunchConfiguration('target_parts_type')
     _target_parts_name = LaunchConfiguration('target_parts_name')
+    _enable_tf_micom = LaunchConfiguration('micom.enable_tf')
     _scan = LaunchConfiguration('scan')
     _cmd_vel = LaunchConfiguration('cmd_vel')
     _odom = LaunchConfiguration('odom')
     _imu = LaunchConfiguration('imu')
     _navsatfix = LaunchConfiguration('navsatfix')
 
-    cloisim_ros_cmd = Node(
-        package="cloisim_ros_bringup",
-        executable="bringup",
-        output='screen',
-        remappings=[('scan', _scan),
-                    ('cmd_vel', _cmd_vel),
-                    ('odom', _odom),
-                    ('imu', _imu),
-                    ('navsatfix', _navsatfix)],
-        parameters=[{'single_mode': _single_mode,
-                     'target_model': ParameterValue(_target_model, value_type=str),
-                     'target_parts_type': ParameterValue(_target_parts_type, value_type=str),
-                     'target_parts_name': ParameterValue(_target_parts_name, value_type=str),
-                     }])
-
     declare_launch_argument_sm = DeclareLaunchArgument(
         'single_mode',
         default_value='False',
@@ -62,6 +46,11 @@ def generate_launch_description():
         default_value='',
         description='specify the name of target parts you want')
 
+    declare_launch_argument_etm = DeclareLaunchArgument(
+        'micom.enable_tf',
+        default_value='True',
+        description='whether to use tf/tf_static for Micom')
+
     declare_launch_argument_sc = DeclareLaunchArgument(
         'scan',
         default_value='scan',
@@ -71,17 +60,17 @@ def generate_launch_description():
         'cmd_vel',
         default_value='cmd_vel',
         description='specify cmd_vel topic you want')
-    
+
     declare_launch_argument_odom = DeclareLaunchArgument(
         'odom',
         default_value='odom',
         description='specify odom topic you want')
-    
+
     declare_launch_argument_imu = DeclareLaunchArgument(
         'imu',
         default_value='imu',
         description='specify imu/data topic you want')
-    
+
     declare_launch_argument_navsatfix = DeclareLaunchArgument(
         'navsatfix',
         default_value='navsatfix',
@@ -93,6 +82,21 @@ def generate_launch_description():
     stdout_log_buf_stream_envvar = SetEnvironmentVariable(
         'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
 
+    cloisim_ros_cmd = Node(
+        package="cloisim_ros_bringup",
+        executable="bringup",
+        output='screen',
+        remappings=[('scan', _scan),
+                    ('cmd_vel', _cmd_vel),
+                    ('odom', _odom),
+                    ('imu', _imu),
+                    ('navsatfix', _navsatfix)],
+        parameters=[{'single_mode': _single_mode,
+                     'target_model': ParameterValue(_target_model, value_type=str),
+                     'target_parts_type': ParameterValue(_target_parts_type, value_type=str),
+                     'target_parts_name': ParameterValue(_target_parts_name, value_type=str),
+                     'micom.enable_tf': _enable_tf_micom}])
+
     # Create the launch description and populate
     ld = launch.LaunchDescription()
 
@@ -108,6 +112,7 @@ def generate_launch_description():
     ld.add_action(declare_launch_argument_odom)
     ld.add_action(declare_launch_argument_imu)
     ld.add_action(declare_launch_argument_navsatfix)
+    ld.add_action(declare_launch_argument_etm)
     ld.add_action(cloisim_ros_cmd)
 
     return ld