From b284ad2202512778853e3194df3823bf806d573d Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 24 Jun 2024 18:45:15 +0900 Subject: [PATCH] Make it possible to switch TF/TF_static publishing for micom from cloisim_ros_bringup --- cloisim_ros_base/src/base.cpp | 3 +- cloisim_ros_bringup/README.md | 9 ++ cloisim_ros_bringup/launch/bringup.launch.py | 114 +---------------- cloisim_ros_bringup/launch/bringup_launch.py | 118 ++++++++++++++++++ cloisim_ros_bringup/launch/cloisim.launch.py | 43 +------ cloisim_ros_bringup/launch/cloisim_launch.py | 43 +++++++ cloisim_ros_bringup/src/bringup.cpp | 54 ++++---- cloisim_ros_bringup/src/main.cpp | 2 +- .../bringup_param.hpp | 15 ++- .../src/bringup_param.cpp | 76 +++++++++-- cloisim_ros_micom/src/main.cpp | 5 + 11 files changed, 284 insertions(+), 198 deletions(-) mode change 100644 => 120000 cloisim_ros_bringup/launch/bringup.launch.py create mode 100644 cloisim_ros_bringup/launch/bringup_launch.py mode change 100644 => 120000 cloisim_ros_bringup/launch/cloisim.launch.py create mode 100644 cloisim_ros_bringup/launch/cloisim_launch.py diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index 96e63462..34c199ca 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/cloisim_ros_base/src/base.cpp @@ -52,6 +52,7 @@ Base::Base(const string node_name, const string namespace_, const rclcpp::NodeOp , m_node_handle(shared_ptr(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)); } @@ -75,7 +76,7 @@ void Base::Start(const bool enable_tf_publish) Initialize(); - DBG_SIM_MSG("enable_tf(%d)", enable_tf_publish_); + DBG_SIM_MSG("node(%s) enable_tf(%d)", get_name(), enable_tf_publish_); auto callback_static_tf_pub = [this]() -> void { diff --git a/cloisim_ros_bringup/README.md b/cloisim_ros_bringup/README.md index 895b4f37..07b818ef 100644 --- a/cloisim_ros_bringup/README.md +++ b/cloisim_ros_bringup/README.md @@ -64,3 +64,12 @@ Specify target model from simulation ```shell ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True target_model:=cloi0 ``` + +### Enable or disable TF/TF_Static publishing + +Currently only support "micom" type in cloisim_ros_bringup. + +```shell +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 +``` diff --git a/cloisim_ros_bringup/launch/bringup.launch.py b/cloisim_ros_bringup/launch/bringup.launch.py deleted file mode 100644 index 977d1336..00000000 --- a/cloisim_ros_bringup/launch/bringup.launch.py +++ /dev/null @@ -1,113 +0,0 @@ -# -# LGE Advanced Robotics Laboratory -# Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea -# All Rights are Reserved. -# -# SPDX-License-Identifier: MIT -# - -import os -import launch.actions -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.actions import SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.parameter_descriptions import ParameterValue - - -def generate_launch_description(): - - _single_mode = LaunchConfiguration('single_mode') - _target_model = LaunchConfiguration('target_model') - _target_parts_type = LaunchConfiguration('target_parts_type') - _target_parts_name = LaunchConfiguration('target_parts_name') - _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', - description='whether to use single mode') - - declare_launch_argument_tm = DeclareLaunchArgument( - 'target_model', - default_value='', - description='specify the target model you want') - - declare_launch_argument_tpt = DeclareLaunchArgument( - 'target_parts_type', - default_value='', - description='specify the type of target parts you want') - - declare_launch_argument_tpn = DeclareLaunchArgument( - 'target_parts_name', - default_value='', - description='specify the name of target parts you want') - - declare_launch_argument_sc = DeclareLaunchArgument( - 'scan', - default_value='scan', - description='specify scan topic you want') - - declare_launch_argument_cmdvel = DeclareLaunchArgument( - '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', - description='specify navsatfix topic you want') - - stdout_log_use_stdout_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_USE_STDOUT', '1') - - stdout_log_buf_stream_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') - - # Create the launch description and populate - ld = launch.LaunchDescription() - - # Set environment variables - ld.add_action(stdout_log_use_stdout_envvar) - ld.add_action(stdout_log_buf_stream_envvar) - ld.add_action(declare_launch_argument_sm) - ld.add_action(declare_launch_argument_tm) - ld.add_action(declare_launch_argument_tpt) - ld.add_action(declare_launch_argument_tpn) - ld.add_action(declare_launch_argument_sc) - ld.add_action(declare_launch_argument_cmdvel) - ld.add_action(declare_launch_argument_odom) - ld.add_action(declare_launch_argument_imu) - ld.add_action(declare_launch_argument_navsatfix) - ld.add_action(cloisim_ros_cmd) - - return ld diff --git a/cloisim_ros_bringup/launch/bringup.launch.py b/cloisim_ros_bringup/launch/bringup.launch.py new file mode 120000 index 00000000..6d270b20 --- /dev/null +++ b/cloisim_ros_bringup/launch/bringup.launch.py @@ -0,0 +1 @@ +bringup_launch.py \ No newline at end of file diff --git a/cloisim_ros_bringup/launch/bringup_launch.py b/cloisim_ros_bringup/launch/bringup_launch.py new file mode 100644 index 00000000..d31a5e76 --- /dev/null +++ b/cloisim_ros_bringup/launch/bringup_launch.py @@ -0,0 +1,118 @@ +""" +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, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + + _single_mode = LaunchConfiguration('single_mode') + _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') + + declare_launch_argument_sm = DeclareLaunchArgument( + 'single_mode', + default_value='False', + description='whether to use single mode') + + declare_launch_argument_tm = DeclareLaunchArgument( + 'target_model', + default_value='', + description='specify the target model you want') + + declare_launch_argument_tpt = DeclareLaunchArgument( + 'target_parts_type', + default_value='', + description='specify the type of target parts you want') + + declare_launch_argument_tpn = DeclareLaunchArgument( + 'target_parts_name', + 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', + description='specify scan topic you want') + + declare_launch_argument_cmdvel = DeclareLaunchArgument( + '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', + description='specify navsatfix topic you want') + + stdout_log_use_stdout_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_USE_STDOUT', '1') + + 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() + + # Set environment variables + ld.add_action(stdout_log_use_stdout_envvar) + ld.add_action(stdout_log_buf_stream_envvar) + ld.add_action(declare_launch_argument_sm) + ld.add_action(declare_launch_argument_tm) + ld.add_action(declare_launch_argument_tpt) + ld.add_action(declare_launch_argument_tpn) + ld.add_action(declare_launch_argument_sc) + ld.add_action(declare_launch_argument_cmdvel) + 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 diff --git a/cloisim_ros_bringup/launch/cloisim.launch.py b/cloisim_ros_bringup/launch/cloisim.launch.py deleted file mode 100644 index d75226c9..00000000 --- a/cloisim_ros_bringup/launch/cloisim.launch.py +++ /dev/null @@ -1,42 +0,0 @@ -# -# LGE Advanced Robotics Laboratory -# Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea -# All Rights are Reserved. -# -# SPDX-License-Identifier: MIT -# -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.substitutions import LaunchConfiguration - - -def generate_launch_description() -> LaunchDescription: - - sim_path = LaunchConfiguration('sim_path') - world = LaunchConfiguration('world') - - execute_multi_robot_simulator = ExecuteProcess( - cmd=['./CLOiSim.x86_64', '-world', world], - cwd=[sim_path], - output='screen') - - declare_launch_argument_sim_path = DeclareLaunchArgument( - 'sim_path', - default_value='', - description='path where the CLOiSim simulator located') - - declare_launch_argument_world = DeclareLaunchArgument( - 'world', - default_value='', - description='It is World file name. Please check environments before run. [CLOISIM_WORLD_PATH, CLOISIM_MODEL_PATH]') - - # Create the launch description and populate - ld = LaunchDescription() - - # Add the actions to launch all nodes - ld.add_action(declare_launch_argument_sim_path) - ld.add_action(declare_launch_argument_world) - ld.add_action(execute_multi_robot_simulator) - - return ld diff --git a/cloisim_ros_bringup/launch/cloisim.launch.py b/cloisim_ros_bringup/launch/cloisim.launch.py new file mode 120000 index 00000000..6f7e1a42 --- /dev/null +++ b/cloisim_ros_bringup/launch/cloisim.launch.py @@ -0,0 +1 @@ +cloisim_launch.py \ No newline at end of file diff --git a/cloisim_ros_bringup/launch/cloisim_launch.py b/cloisim_ros_bringup/launch/cloisim_launch.py new file mode 100644 index 00000000..d9dbdc48 --- /dev/null +++ b/cloisim_ros_bringup/launch/cloisim_launch.py @@ -0,0 +1,43 @@ +""" +LGE Advanced Robotics Laboratory +Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea +All Rights are Reserved. + +SPDX-License-Identifier: MIT +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + + sim_path = LaunchConfiguration('sim_path') + world = LaunchConfiguration('world') + + execute_multi_robot_simulator = ExecuteProcess( + cmd=['./CLOiSim.x86_64', '-world', world], + cwd=[sim_path], + output='screen') + + declare_launch_argument_sim_path = DeclareLaunchArgument( + 'sim_path', + default_value='', + description='path where the CLOiSim simulator located') + + declare_launch_argument_world = DeclareLaunchArgument( + 'world', + default_value='', + description='It is World file name. Please check environments before run. [CLOISIM_WORLD_PATH, CLOISIM_MODEL_PATH]') + + # Create the launch description and populate + ld = LaunchDescription() + + # Add the actions to launch all nodes + ld.add_action(declare_launch_argument_sim_path) + ld.add_action(declare_launch_argument_world) + ld.add_action(execute_multi_robot_simulator) + + return ld diff --git a/cloisim_ros_bringup/src/bringup.cpp b/cloisim_ros_bringup/src/bringup.cpp index 7fb1f54a..28021cc6 100644 --- a/cloisim_ros_bringup/src/bringup.cpp +++ b/cloisim_ros_bringup/src/bringup.cpp @@ -35,92 +35,96 @@ using namespace std; map, tuple>> g_node_map_list; // static vector> loaded_key_list; -static bool enable_single_mode = false; +static bool g_enable_single_mode = false; static shared_ptr make_device_node( rclcpp::NodeOptions& node_options, const string& node_type, const string& model_name, - const string& node_name) + const string& node_name, + const Json::Value& node_param) { shared_ptr node = nullptr; - if (enable_single_mode) + if (g_enable_single_mode) node_options.append_parameter_override("single_mode.robotname", model_name); if (!node_type.compare("MICOM")) { - if (enable_single_mode) + if (!node_param["enable_tf"].isNull()) + node_options.append_parameter_override("enable_tf", node_param["enable_tf"].asBool()); + + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("JOINTCONTROL")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("LIDAR") || !node_type.compare("LASER")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("CAMERA")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("DEPTHCAMERA")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("MULTICAMERA")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("REALSENSE")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("SEGMENTCAMERA")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("GPS")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("IMU")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); } else if (!node_type.compare("SONAR")) { - if (enable_single_mode) + if (g_enable_single_mode) node = std::make_shared(node_options, node_name); else node = std::make_shared(node_options, node_name, model_name); @@ -155,7 +159,7 @@ static shared_ptr make_world_node( } static void parse_target_parts_by_name( - const Json::Value item, + const Json::Value& item, const string node_type, const string model_name, const string node_name) @@ -163,15 +167,13 @@ static void parse_target_parts_by_name( shared_ptr node = nullptr; rclcpp::NodeOptions node_options; - node_options.append_parameter_override("single_mode", static_cast(enable_single_mode)); + node_options.append_parameter_override("single_mode", static_cast(g_enable_single_mode)); cloisim_ros::BringUpParam::StoreBridgeInfosAsParameters(item, node_options); - const auto node_info = "Target Model/Type/Parts: " + model_name + "/" + node_type + "/" + node_name; - const auto key = tie(model_name, node_type, node_name); - loaded_key_list.push_back(key); + // const auto node_info = "Target Model/Type/Parts: " + model_name + "/" + node_type + "/" + node_name; // cout << "node info: " << node_info << endl; if (g_node_map_list.find(key) != g_node_map_list.end()) { @@ -180,11 +182,13 @@ static void parse_target_parts_by_name( } if (cloisim_ros::BringUpParam::IsRobotSpecificType(node_type)) - node = make_device_node(node_options, node_type, model_name, node_name); + node = make_device_node(node_options, node_type, model_name, node_name, item); + else if (cloisim_ros::BringUpParam::IsWorldSpecificType(node_type)) node = make_world_node(node_options, node_type, model_name, node_name); + else - cout << node_type << " is not supported." << endl; + cout << node_type << " is NOT supported." << endl; if (node != nullptr) { @@ -194,7 +198,7 @@ static void parse_target_parts_by_name( } static void parse_target_parts_by_type( - const Json::Value node_list, + const Json::Value& node_list, const string node_type, const string model_name, const string targetPartsName) @@ -217,7 +221,7 @@ static void parse_target_parts_by_type( } static void parse_target_model( - const Json::Value item_list, + const Json::Value& item_list, const string item_name, const string targetPartsType, const string targetPartsName) @@ -237,13 +241,13 @@ static void parse_target_model( } void make_bringup_list( - const Json::Value result_map, + const Json::Value& result_map, const string target_model, const string target_parts_type, const string target_parts_name, const bool is_single_mode) { - enable_single_mode = is_single_mode; + g_enable_single_mode = is_single_mode; loaded_key_list.clear(); for (auto it = result_map.begin(); it != result_map.end(); it++) diff --git a/cloisim_ros_bringup/src/main.cpp b/cloisim_ros_bringup/src/main.cpp index 7edec10e..7929854a 100644 --- a/cloisim_ros_bringup/src/main.cpp +++ b/cloisim_ros_bringup/src/main.cpp @@ -21,7 +21,7 @@ using namespace std; extern map, tuple>> g_node_map_list; -void make_bringup_list(const Json::Value result_map, const string target_model, const string target_parts_type, const string target_parts_name, const bool is_single_mode); +void make_bringup_list(const Json::Value& result_map, const string target_model, const string target_parts_type, const string target_parts_name, const bool is_single_mode); void remove_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger& logger) { diff --git a/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp b/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp index ea75bd3d..738a03e0 100644 --- a/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp +++ b/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp @@ -1,6 +1,6 @@ /** * @file bringup_param.hpp - * @date 2021-01-21 + * @date 2024-06-24 * @author Hyunseok Yang * @brief * load parameters for bringup @@ -46,23 +46,26 @@ class BringUpParam : public rclcpp::Node void TargetPartsName(const string value) { target_parts_name = value; } Json::Value GetBringUpList(const bool filterByParameters = false); - - void StoreFilteredInfoAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options); - - public: Json::Value RequestBringUpList(); static bool IsRobotSpecificType(const string node_type); - static bool IsWorldSpecificType(const string node_type); + void StoreFilteredInfoAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options); static void StoreBridgeInfosAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options); + bool HasEnableTFforMicom() { return has_parameter("micom.enable_tf"); } + bool EnableTFforMicom() { return enable_tf_micom_; } + +private: + void SetIndividualParameters(Json::Value& result); + private: bool is_single_mode; string target_model; string target_parts_type; string target_parts_name; + bool enable_tf_micom_; WebSocketService *ws_service_ptr_; diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp index af407646..50f28ceb 100644 --- a/cloisim_ros_bringup_param/src/bringup_param.cpp +++ b/cloisim_ros_bringup_param/src/bringup_param.cpp @@ -1,6 +1,6 @@ /** - * @file main.cpp - * @date 2021-01-16 + * @file bringup_param.cpp + * @date 2024-06-24 * @author Hyunseok Yang * @brief * @remark @@ -43,6 +43,7 @@ BringUpParam::BringUpParam(const string basename) , target_model("") , target_parts_type("") , target_parts_name("") + , enable_tf_micom_(true) , ws_service_ptr_(nullptr) { get_parameter("single_mode", is_single_mode); @@ -50,9 +51,18 @@ BringUpParam::BringUpParam(const string basename) get_parameter("target_parts_type", target_parts_type); get_parameter("target_parts_name", target_parts_name); + if (HasEnableTFforMicom()) + { + get_parameter("micom.enable_tf", enable_tf_micom_); + } + RCLCPP_INFO_ONCE(this->get_logger(), "Params > single_mode(%d) target_model(%s) target_parts_type(%s) target_parts_name(%s)", is_single_mode, target_model.c_str(), target_parts_type.c_str(), target_parts_name.c_str()); + + RCLCPP_INFO_ONCE(this->get_logger(), + "enable_tf > micom(%d)", + (int)enable_tf_micom_); } BringUpParam::~BringUpParam() @@ -110,6 +120,7 @@ Json::Value BringUpParam::RequestBringUpList() if (root["result"].size() > 0) { result = root["result"]; + SetIndividualParameters(result); // cout << "There is node map list: " << result.size() << endl; // cout << result << endl; break; @@ -125,6 +136,51 @@ Json::Value BringUpParam::RequestBringUpList() return result; } +void BringUpParam::SetIndividualParameters(Json::Value& result) +{ + if (result.isNull()) + return; + + // cout << __FUNCTION__<< endl; + // cout << result << endl; + for (auto it = result.begin(); it != result.end(); it++) + { + const auto node_namespace = it.key().asString(); + auto item_list = (*it); + // cout << node_namespace << endl; + + for (auto it2 = item_list.begin(); it2 != item_list.end(); ++it2) + { + const auto node_type = it2.key().asString(); + auto node_list = (*it2); + + // cout << "\t" << node_type << ", " << endl; + if (node_type.compare("MICOM") == 0) + { + for (auto it3 = node_list.begin(); it3 != node_list.end(); ++it3) + { + const auto node_name = it3.key().asString(); + auto node_param_list = (*it3); + + if (HasEnableTFforMicom()) + { + node_param_list["enable_tf"] = EnableTFforMicom(); + } + + // cout << "\t\t node_name: " << node_name << endl; + // for (auto it4 = node_param_list.begin(); it4 != node_param_list.end(); ++it4) + // cout << "\t\t\t" << it4.key().asString() << ": " << (*it4).asString() << endl; + + node_list[node_name] = node_param_list; + } + } + item_list[node_type] = node_list; + } + result[node_namespace] = item_list; + } + // cout << result << endl; +} + Json::Value BringUpParam::GetFilteredListByParameters(const Json::Value result) { Json::Value root; @@ -167,15 +223,15 @@ void BringUpParam::StoreBridgeInfosAsParameters( const Json::Value item, rclcpp::NodeOptions &node_options) { - if (!item.isNull()) + if (item.isNull()) + return; + + for (auto it = item.begin(); it != item.end(); ++it) { - for (auto it = item.begin(); it != item.end(); ++it) - { - const auto bridge_key = it.key().asString(); - const auto bridge_port = (*it).asUInt(); - node_options.append_parameter_override("bridge." + bridge_key, uint16_t(bridge_port)); - // cout << "bridge." << bridge_key << " = " << bridge_port << endl; - } + const auto bridge_key = it.key().asString(); + const auto bridge_port = (*it).asUInt(); + node_options.append_parameter_override("bridge." + bridge_key, uint16_t(bridge_port)); + // cout << "bridge." << bridge_key << " = " << bridge_port << endl; } } diff --git a/cloisim_ros_micom/src/main.cpp b/cloisim_ros_micom/src/main.cpp index f5bb5c92..6cac08e2 100644 --- a/cloisim_ros_micom/src/main.cpp +++ b/cloisim_ros_micom/src/main.cpp @@ -37,6 +37,11 @@ int main(int argc, char** argv) const auto model_name = bringup_param_node->TargetModel(); const auto node_name = bringup_param_node->TargetPartsName(); + if (bringup_param_node->HasEnableTFforMicom()) + { + node_options.append_parameter_override("enable_tf", bringup_param_node->EnableTFforMicom()); + } + if (is_single_mode) node = std::make_shared(node_options, node_name); else