From 51880ef0ab524b4a9aa0c4c68bf335e368569129 Mon Sep 17 00:00:00 2001 From: techlabs Date: Tue, 23 Aug 2022 18:56:23 +0200 Subject: [PATCH 01/14] investigation on the use of arguments for context creation: remapping, namespace and params are passed through context --- .../src/ign_ros2_control_plugin.cpp | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 951e631f..be22369a 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -95,6 +95,9 @@ class IgnitionROS2ControlPluginPrivate // TODO(ahcorde): Add param in plugin tag std::string robot_description_node_ = "robot_state_publisher"; + /// \brief String with the namesapce of the robot to cascade to all sub-nodes + std::string robot_namespace_ = "" + /// \brief Last time the update method was called rclcpp::Time last_update_sim_time_ros_ = rclcpp::Time((int64_t)0, RCL_ROS_TIME); @@ -291,30 +294,42 @@ void IgnitionROS2ControlPlugin::Configure( controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } + // TODO Figure out if this is used, if yes it means we will require the use of the context, otherwise remove altogether + std::vector arguments = {"--ros-args", "--params-file", paramFileName}; + auto sdfPtr = const_cast(_sdf.get()); + if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); - // Set namespace if tag is present + // Set namespace if tag is present if (sdfRos->HasElement("namespace")) { std::string ns = sdfRos->GetElement("namespace")->Get(); + std::cout << "#################### DEBUG namespace:" << ns << std::endl; // prevent exception: namespace must be absolute, it must lead with a '/' if (ns.empty() || ns[0] != '/') { ns = '/' + ns; } - std::string ns_arg = std::string("__ns:=") + ns; - arguments.push_back(RCL_REMAP_FLAG); - arguments.push_back(ns_arg); + if(ns.length()>1) { + this->dataPtr->namespace_ = ns; + this->dataPtr->robot_description_node_ = ns + "/robot_state_publisher"; + } + + // TODO Figure out if this is used, if yes it means we will require the use of the context, otherwise remove altogether + //std::string ns_arg = std::string("__ns:=") + ns; + //arguments.push_back(RCL_REMAP_FLAG); + //arguments.push_back(ns_arg); } // Get list of remapping rules from SDF if (sdfRos->HasElement("remapping")) { sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); - arguments.push_back(RCL_ROS_ARGS_FLAG); + // TODO Figure out if this is used, if yes it means we will require the use of the context, otherwise remove altogether + // arguments.push_back(RCL_ROS_ARGS_FLAG); while (argument_sdf) { std::string argument = argument_sdf->Get(); - arguments.push_back(RCL_REMAP_FLAG); - arguments.push_back(argument); + // arguments.push_back(RCL_REMAP_FLAG); + // arguments.push_back(argument); argument_sdf = argument_sdf->GetNextElement("remapping"); } } From 49998e02cf40096f12e82fd23ef8507991fba156 Mon Sep 17 00:00:00 2001 From: techlabs Date: Thu, 25 Aug 2022 19:13:22 +0200 Subject: [PATCH 02/14] removing node name prefix and applying namespace changes --- .../src/ign_ros2_control_plugin.cpp | 66 ++++++++----------- 1 file changed, 26 insertions(+), 40 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index be22369a..7a5ce414 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -87,6 +87,9 @@ class IgnitionROS2ControlPluginPrivate std::shared_ptr controller_manager_{nullptr}; + /// \brief String with the name of the controller manager node + std::string controller_manager_node_ = "controller_manager"; + /// \brief String with the robot description param_name // TODO(ahcorde): Add param in plugin tag std::string robot_description_ = "robot_description"; @@ -95,7 +98,7 @@ class IgnitionROS2ControlPluginPrivate // TODO(ahcorde): Add param in plugin tag std::string robot_description_node_ = "robot_state_publisher"; - /// \brief String with the namesapce of the robot to cascade to all sub-nodes + /// \brief String with the namespace of the robot to cascade to all sub-nodes std::string robot_namespace_ = "" /// \brief Last time the update method was called @@ -252,6 +255,11 @@ void IgnitionROS2ControlPlugin::Configure( ignition::gazebo::EntityComponentManager & _ecm, ignition::gazebo::EventManager &) { + // Create a default context, if not already + if (!rclcpp::ok()) { + rclcpp::init(); + } + // Make sure the controller is attached to a valid model const auto model = ignition::gazebo::Model(_entity); if (!model.Valid(_ecm)) { @@ -264,6 +272,7 @@ void IgnitionROS2ControlPlugin::Configure( } // Get params from SDF + // TODO check if this is used. This used to be passed to the default context std::string paramFileName = _sdf->Get("parameters"); if (paramFileName.empty()) { @@ -273,10 +282,8 @@ void IgnitionROS2ControlPlugin::Configure( return; } - std::vector arguments = {"--ros-args"}; - + std::vector arguments = {"--ros-args", "--params-file", paramFileName}; auto sdfPtr = const_cast(_sdf.get()); - sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { std::string argument = argument_sdf->Get(); @@ -294,60 +301,38 @@ void IgnitionROS2ControlPlugin::Configure( controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } - // TODO Figure out if this is used, if yes it means we will require the use of the context, otherwise remove altogether - std::vector arguments = {"--ros-args", "--params-file", paramFileName}; - auto sdfPtr = const_cast(_sdf.get()); - if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); // Set namespace if tag is present if (sdfRos->HasElement("namespace")) { std::string ns = sdfRos->GetElement("namespace")->Get(); - std::cout << "#################### DEBUG namespace:" << ns << std::endl; // prevent exception: namespace must be absolute, it must lead with a '/' if (ns.empty() || ns[0] != '/') { ns = '/' + ns; } if(ns.length()>1) { - this->dataPtr->namespace_ = ns; + this->dataPtr->robot_namespace_ = ns; + this->dataPtr->robot_description_ = ns + "/robot_description"; this->dataPtr->robot_description_node_ = ns + "/robot_state_publisher"; - } - - // TODO Figure out if this is used, if yes it means we will require the use of the context, otherwise remove altogether - //std::string ns_arg = std::string("__ns:=") + ns; - //arguments.push_back(RCL_REMAP_FLAG); - //arguments.push_back(ns_arg); - } - - // Get list of remapping rules from SDF - if (sdfRos->HasElement("remapping")) { - sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); - - // TODO Figure out if this is used, if yes it means we will require the use of the context, otherwise remove altogether - // arguments.push_back(RCL_ROS_ARGS_FLAG); - while (argument_sdf) { - std::string argument = argument_sdf->Get(); - // arguments.push_back(RCL_REMAP_FLAG); - // arguments.push_back(argument); - argument_sdf = argument_sdf->GetNextElement("remapping"); + this->dataPtr->controller_manager_node_ = ns + "/controller_manager"; } } } - std::vector argv; - for (const auto & arg : arguments) { - argv.push_back(reinterpret_cast(arg.data())); - } + // Get list of remapping rules from SDF + // TODO check if this is used. This used to be passed to the default context + if (sdfRos->HasElement("remapping")) { + sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); - if (!rclcpp::ok()) { - rclcpp::init(static_cast(argv.size()), argv.data()); - std::string node_name = "ignition_ros_control"; - if (!controllerManagerPrefixNodeName.empty()) { - node_name = controllerManagerPrefixNodeName + "_" + node_name; + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + argument_sdf = argument_sdf->GetNextElement("remapping"); } - this->dataPtr->node_ = rclcpp::Node::make_shared(node_name); } + + // Instanciate and start node with namespace + this->dataPtr->node_ = rclcpp::Node::make_shared("ignition_ros_control", this->dataPtr->robot_namespace_); this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->node_); this->dataPtr->stop_ = false; @@ -436,7 +421,8 @@ void IgnitionROS2ControlPlugin::Configure( new controller_manager::ControllerManager( std::move(resource_manager_), this->dataPtr->executor_, - controllerManagerNodeName)); + this->dataPtr->controller_manager_node_, + this->dataPtr->robot_namespace_)); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) { From c8a6764c6f00f2fcc6e8ff2fb23b48086a75b988 Mon Sep 17 00:00:00 2001 From: techlabs Date: Fri, 26 Aug 2022 15:25:23 +0200 Subject: [PATCH 03/14] fixing compilation errors and reverting some changes --- .../src/ign_ros2_control_plugin.cpp | 39 ++++++++++++------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 7a5ce414..c68b1420 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -99,7 +99,7 @@ class IgnitionROS2ControlPluginPrivate std::string robot_description_node_ = "robot_state_publisher"; /// \brief String with the namespace of the robot to cascade to all sub-nodes - std::string robot_namespace_ = "" + std::string robot_namespace_ = ""; /// \brief Last time the update method was called rclcpp::Time last_update_sim_time_ros_ = @@ -255,10 +255,6 @@ void IgnitionROS2ControlPlugin::Configure( ignition::gazebo::EntityComponentManager & _ecm, ignition::gazebo::EventManager &) { - // Create a default context, if not already - if (!rclcpp::ok()) { - rclcpp::init(); - } // Make sure the controller is attached to a valid model const auto model = ignition::gazebo::Model(_entity); @@ -272,8 +268,8 @@ void IgnitionROS2ControlPlugin::Configure( } // Get params from SDF - // TODO check if this is used. This used to be passed to the default context std::string paramFileName = _sdf->Get("parameters"); + std::vector arguments = {"--ros-args", "--params-file", paramFileName}; if (paramFileName.empty()) { RCLCPP_ERROR( @@ -282,7 +278,6 @@ void IgnitionROS2ControlPlugin::Configure( return; } - std::vector arguments = {"--ros-args", "--params-file", paramFileName}; auto sdfPtr = const_cast(_sdf.get()); sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { @@ -299,8 +294,13 @@ void IgnitionROS2ControlPlugin::Configure( _sdf->Get("controller_manager_prefix_node_name"); if (!controllerManagerPrefixNodeName.empty()) { controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; + + std::vector argv; + for (const auto & arg : arguments) { + argv.push_back(reinterpret_cast(arg.data())); } + auto sdfPtr = const_cast(_sdf.get()); if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); @@ -318,17 +318,26 @@ void IgnitionROS2ControlPlugin::Configure( this->dataPtr->controller_manager_node_ = ns + "/controller_manager"; } } + + // Get list of remapping rules from SDF + if (sdfRos->HasElement("remapping")) { + sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); + arguments.push_back(RCL_ROS_ARGS_FLAG); + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("remapping"); + } + } } - // Get list of remapping rules from SDF - // TODO check if this is used. This used to be passed to the default context - if (sdfRos->HasElement("remapping")) { - sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); - while (argument_sdf) { - std::string argument = argument_sdf->Get(); - argument_sdf = argument_sdf->GetNextElement("remapping"); - } + // Create a default context, if not already + // This only triggers for the first robot launch, + // following robots will not see their params passed to the context + if (!rclcpp::ok()) { + rclcpp::init(static_cast(argv.size()), argv.data()); } // Instanciate and start node with namespace From 7ce4a8bf8e9e2df00bff281b9b350d9b79f88e19 Mon Sep 17 00:00:00 2001 From: techlabs Date: Fri, 26 Aug 2022 17:38:33 +0200 Subject: [PATCH 04/14] removing useless node name prefixing & usage of 'this->' in private object --- .../src/ign_ros2_control_plugin.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index c68b1420..dc43f1bb 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -181,7 +181,6 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( std::string IgnitionROS2ControlPluginPrivate::getURDF() const { std::string urdf_string; - using namespace std::chrono_literals; auto parameters_client = std::make_shared( node_, robot_description_node_); @@ -200,16 +199,16 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const RCLCPP_INFO( node_->get_logger(), "connected to service!! %s asking for %s", robot_description_node_.c_str(), - this->robot_description_.c_str()); + robot_description_.c_str()); // search and wait for robot_description on param server while (urdf_string.empty()) { RCLCPP_DEBUG( node_->get_logger(), "param_name %s", - this->robot_description_.c_str()); + robot_description_.c_str()); try { - auto f = parameters_client->get_parameters({this->robot_description_}); + auto f = parameters_client->get_parameters({robot_description_}); f.wait(); std::vector values = f.get(); urdf_string = values[0].as_string(); @@ -223,7 +222,7 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const RCLCPP_ERROR( node_->get_logger(), "ign_ros2_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", - this->robot_description_.c_str()); + robot_description_.c_str()); } usleep(100000); } @@ -313,9 +312,12 @@ void IgnitionROS2ControlPlugin::Configure( } if(ns.length()>1) { this->dataPtr->robot_namespace_ = ns; - this->dataPtr->robot_description_ = ns + "/robot_description"; this->dataPtr->robot_description_node_ = ns + "/robot_state_publisher"; - this->dataPtr->controller_manager_node_ = ns + "/controller_manager"; + + // TODO check if node names need to be prefixed + //ns.erase(0, 1); //removing '/' for node names + //this->dataPtr->robot_description_ = ns + "_robot_description"; + //this->dataPtr->controller_manager_node_ = ns + "_controller_manager"; } } @@ -369,6 +371,7 @@ void IgnitionROS2ControlPlugin::Configure( return; } + // Read urdf from ros parameter server then // setup actuators and mechanism control node. // This call will block if ROS is not properly initialized. From 9cc290b7258ffcc6ec4b45a987e9cc9e58333c69 Mon Sep 17 00:00:00 2001 From: techlabs Date: Fri, 4 Nov 2022 17:53:05 +0100 Subject: [PATCH 05/14] remove useless comments --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index dc43f1bb..93166713 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -254,7 +254,6 @@ void IgnitionROS2ControlPlugin::Configure( ignition::gazebo::EntityComponentManager & _ecm, ignition::gazebo::EventManager &) { - // Make sure the controller is attached to a valid model const auto model = ignition::gazebo::Model(_entity); if (!model.Valid(_ecm)) { @@ -310,14 +309,9 @@ void IgnitionROS2ControlPlugin::Configure( if (ns.empty() || ns[0] != '/') { ns = '/' + ns; } - if(ns.length()>1) { + if (ns.length() > 1) { this->dataPtr->robot_namespace_ = ns; this->dataPtr->robot_description_node_ = ns + "/robot_state_publisher"; - - // TODO check if node names need to be prefixed - //ns.erase(0, 1); //removing '/' for node names - //this->dataPtr->robot_description_ = ns + "_robot_description"; - //this->dataPtr->controller_manager_node_ = ns + "_controller_manager"; } } @@ -334,10 +328,7 @@ void IgnitionROS2ControlPlugin::Configure( } } - // Create a default context, if not already - // This only triggers for the first robot launch, - // following robots will not see their params passed to the context if (!rclcpp::ok()) { rclcpp::init(static_cast(argv.size()), argv.data()); } @@ -371,7 +362,6 @@ void IgnitionROS2ControlPlugin::Configure( return; } - // Read urdf from ros parameter server then // setup actuators and mechanism control node. // This call will block if ROS is not properly initialized. From 495f88b4bed45a7b2d00862d76c5d0615bfb7c37 Mon Sep 17 00:00:00 2001 From: techlabs Date: Tue, 8 Nov 2022 14:31:03 +0100 Subject: [PATCH 06/14] revert this-> changes rename and cleanup variables --- .../src/ign_ros2_control_plugin.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 93166713..eff2ac25 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -87,16 +87,13 @@ class IgnitionROS2ControlPluginPrivate std::shared_ptr controller_manager_{nullptr}; - /// \brief String with the name of the controller manager node - std::string controller_manager_node_ = "controller_manager"; - /// \brief String with the robot description param_name // TODO(ahcorde): Add param in plugin tag std::string robot_description_ = "robot_description"; /// \brief String with the name of the node that contains the robot_description // TODO(ahcorde): Add param in plugin tag - std::string robot_description_node_ = "robot_state_publisher"; + std::string robot_description_node_name_ = "robot_state_publisher"; /// \brief String with the namespace of the robot to cascade to all sub-nodes std::string robot_namespace_ = ""; @@ -181,34 +178,35 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( std::string IgnitionROS2ControlPluginPrivate::getURDF() const { std::string urdf_string; + using namespace std::chrono_literals; auto parameters_client = std::make_shared( - node_, robot_description_node_); + node_, robot_description_node_name_); while (!parameters_client->wait_for_service(0.5s)) { if (!rclcpp::ok()) { RCLCPP_ERROR( node_->get_logger(), "Interrupted while waiting for %s service. Exiting.", - robot_description_node_.c_str()); + robot_description_node_name_.c_str()); return 0; } RCLCPP_ERROR( node_->get_logger(), "%s service not available, waiting again...", - robot_description_node_.c_str()); + robot_description_node_name_.c_str()); } RCLCPP_INFO( node_->get_logger(), "connected to service!! %s asking for %s", - robot_description_node_.c_str(), - robot_description_.c_str()); + robot_description_node_name_.c_str(), + this->robot_description_.c_str()); // search and wait for robot_description on param server while (urdf_string.empty()) { RCLCPP_DEBUG( node_->get_logger(), "param_name %s", - robot_description_.c_str()); + this->robot_description_.c_str()); try { - auto f = parameters_client->get_parameters({robot_description_}); + auto f = parameters_client->get_parameters({this->robot_description_}); f.wait(); std::vector values = f.get(); urdf_string = values[0].as_string(); @@ -222,7 +220,7 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const RCLCPP_ERROR( node_->get_logger(), "ign_ros2_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", - robot_description_.c_str()); + this->robot_description_.c_str()); } usleep(100000); } @@ -311,7 +309,7 @@ void IgnitionROS2ControlPlugin::Configure( } if (ns.length() > 1) { this->dataPtr->robot_namespace_ = ns; - this->dataPtr->robot_description_node_ = ns + "/robot_state_publisher"; + this->dataPtr->robot_description_node_name_ = ns + "/robot_state_publisher"; } } @@ -423,7 +421,7 @@ void IgnitionROS2ControlPlugin::Configure( new controller_manager::ControllerManager( std::move(resource_manager_), this->dataPtr->executor_, - this->dataPtr->controller_manager_node_, + "controller_manager", this->dataPtr->robot_namespace_)); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); From a0bdc451aa2fe4c3423a3afe76843a806277ac4e Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Thu, 17 Nov 2022 16:04:11 +0100 Subject: [PATCH 07/14] re-establishing controllerManagerNodeName prefix --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index eff2ac25..49f1cd73 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -296,11 +296,20 @@ void IgnitionROS2ControlPlugin::Configure( argv.push_back(reinterpret_cast(arg.data())); } + // Get controller manager node name + std::string controllerManagerNodeName{"controller_manager"}; + std::string controllerManagerPrefixNodeName = + _sdf->Get("controller_manager_prefix_node_name"); + if (!controllerManagerPrefixNodeName.empty()) { + controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; + } + auto sdfPtr = const_cast(_sdf.get()); + if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); - // Set namespace if tag is present + // Set namespace if tag is present if (sdfRos->HasElement("namespace")) { std::string ns = sdfRos->GetElement("namespace")->Get(); // prevent exception: namespace must be absolute, it must lead with a '/' @@ -421,7 +430,7 @@ void IgnitionROS2ControlPlugin::Configure( new controller_manager::ControllerManager( std::move(resource_manager_), this->dataPtr->executor_, - "controller_manager", + controllerManagerNodeName, this->dataPtr->robot_namespace_)); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); From 0f6cfcbcf9521f2afa9e49041eff84d2d078f388 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Thu, 17 Nov 2022 17:12:46 +0100 Subject: [PATCH 08/14] fixing line>100char error --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 49f1cd73..df7bcccd 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -341,7 +341,9 @@ void IgnitionROS2ControlPlugin::Configure( } // Instanciate and start node with namespace - this->dataPtr->node_ = rclcpp::Node::make_shared("ignition_ros_control", this->dataPtr->robot_namespace_); + this->dataPtr->node_ = rclcpp::Node::make_shared( + "ignition_ros_control", + this->dataPtr->robot_namespace_); this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->node_); this->dataPtr->stop_ = false; From 72ee5a05478121903559a1be0bb120f3b65f574b Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Tue, 29 Nov 2022 15:01:30 +0100 Subject: [PATCH 09/14] cleanup after rebase --- .../src/ign_ros2_control_plugin.cpp | 23 ++++++------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index df7bcccd..a3d4877c 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -265,7 +265,6 @@ void IgnitionROS2ControlPlugin::Configure( // Get params from SDF std::string paramFileName = _sdf->Get("parameters"); - std::vector arguments = {"--ros-args", "--params-file", paramFileName}; if (paramFileName.empty()) { RCLCPP_ERROR( @@ -274,6 +273,7 @@ void IgnitionROS2ControlPlugin::Configure( return; } + std::vector arguments = {"--ros-args", "--params-file", paramFileName}; auto sdfPtr = const_cast(_sdf.get()); sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { @@ -290,21 +290,6 @@ void IgnitionROS2ControlPlugin::Configure( _sdf->Get("controller_manager_prefix_node_name"); if (!controllerManagerPrefixNodeName.empty()) { controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; - - std::vector argv; - for (const auto & arg : arguments) { - argv.push_back(reinterpret_cast(arg.data())); - } - - // Get controller manager node name - std::string controllerManagerNodeName{"controller_manager"}; - std::string controllerManagerPrefixNodeName = - _sdf->Get("controller_manager_prefix_node_name"); - if (!controllerManagerPrefixNodeName.empty()) { - controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; - } - - auto sdfPtr = const_cast(_sdf.get()); if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); @@ -325,6 +310,7 @@ void IgnitionROS2ControlPlugin::Configure( // Get list of remapping rules from SDF if (sdfRos->HasElement("remapping")) { sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); + arguments.push_back(RCL_ROS_ARGS_FLAG); while (argument_sdf) { std::string argument = argument_sdf->Get(); @@ -335,6 +321,11 @@ void IgnitionROS2ControlPlugin::Configure( } } + std::vector argv; + for (const auto & arg : arguments) { + argv.push_back(reinterpret_cast(arg.data())); + } + // Create a default context, if not already if (!rclcpp::ok()) { rclcpp::init(static_cast(argv.size()), argv.data()); From 72189ccc6c3da924542a8b3e9722c04c4ec9df19 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Tue, 29 Nov 2022 15:07:01 +0100 Subject: [PATCH 10/14] correct small typos --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index a3d4877c..fc2a56ff 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -275,6 +275,7 @@ void IgnitionROS2ControlPlugin::Configure( std::vector arguments = {"--ros-args", "--params-file", paramFileName}; auto sdfPtr = const_cast(_sdf.get()); + sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { std::string argument = argument_sdf->Get(); @@ -290,6 +291,7 @@ void IgnitionROS2ControlPlugin::Configure( _sdf->Get("controller_manager_prefix_node_name"); if (!controllerManagerPrefixNodeName.empty()) { controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; + } if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); From 80c665799cca294f38daf4108c2f104986ed35ed Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Tue, 29 Nov 2022 15:08:24 +0100 Subject: [PATCH 11/14] revert minor lines and spacing --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index fc2a56ff..7933548f 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -274,8 +274,9 @@ void IgnitionROS2ControlPlugin::Configure( } std::vector arguments = {"--ros-args", "--params-file", paramFileName}; + auto sdfPtr = const_cast(_sdf.get()); - + sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { std::string argument = argument_sdf->Get(); From 096b2207518727c8e5359c9620d2588a9dded5cf Mon Sep 17 00:00:00 2001 From: techlabs Date: Wed, 7 Dec 2022 18:48:06 +0100 Subject: [PATCH 12/14] revert renaming and re-establish controllerManagerPrefixNodeName --- .../src/ign_ros2_control_plugin.cpp | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 7933548f..f8190d8f 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -93,10 +93,7 @@ class IgnitionROS2ControlPluginPrivate /// \brief String with the name of the node that contains the robot_description // TODO(ahcorde): Add param in plugin tag - std::string robot_description_node_name_ = "robot_state_publisher"; - - /// \brief String with the namespace of the robot to cascade to all sub-nodes - std::string robot_namespace_ = ""; + std::string robot_description_node_ = "robot_state_publisher"; /// \brief Last time the update method was called rclcpp::Time last_update_sim_time_ros_ = @@ -181,22 +178,22 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const using namespace std::chrono_literals; auto parameters_client = std::make_shared( - node_, robot_description_node_name_); + node_, robot_description_node_); while (!parameters_client->wait_for_service(0.5s)) { if (!rclcpp::ok()) { RCLCPP_ERROR( node_->get_logger(), "Interrupted while waiting for %s service. Exiting.", - robot_description_node_name_.c_str()); + robot_description_node_.c_str()); return 0; } RCLCPP_ERROR( node_->get_logger(), "%s service not available, waiting again...", - robot_description_node_name_.c_str()); + robot_description_node_.c_str()); } RCLCPP_INFO( node_->get_logger(), "connected to service!! %s asking for %s", - robot_description_node_name_.c_str(), + robot_description_node_.c_str(), this->robot_description_.c_str()); // search and wait for robot_description on param server @@ -273,7 +270,7 @@ void IgnitionROS2ControlPlugin::Configure( return; } - std::vector arguments = {"--ros-args", "--params-file", paramFileName}; + std::vector arguments = {"--ros-args"}; auto sdfPtr = const_cast(_sdf.get()); @@ -294,19 +291,19 @@ void IgnitionROS2ControlPlugin::Configure( controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } + std::string ns = "/"; if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); // Set namespace if tag is present if (sdfRos->HasElement("namespace")) { - std::string ns = sdfRos->GetElement("namespace")->Get(); + ns = sdfRos->GetElement("namespace")->Get(); // prevent exception: namespace must be absolute, it must lead with a '/' if (ns.empty() || ns[0] != '/') { ns = '/' + ns; } if (ns.length() > 1) { - this->dataPtr->robot_namespace_ = ns; - this->dataPtr->robot_description_node_name_ = ns + "/robot_state_publisher"; + this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_; } } @@ -334,10 +331,12 @@ void IgnitionROS2ControlPlugin::Configure( rclcpp::init(static_cast(argv.size()), argv.data()); } - // Instanciate and start node with namespace - this->dataPtr->node_ = rclcpp::Node::make_shared( - "ignition_ros_control", - this->dataPtr->robot_namespace_); + std::string node_name = "ignition_ros_control"; + if (!controllerManagerPrefixNodeName.empty()) { + node_name = controllerManagerPrefixNodeName + "_" + node_name; + } + + this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->node_); this->dataPtr->stop_ = false; @@ -427,7 +426,7 @@ void IgnitionROS2ControlPlugin::Configure( std::move(resource_manager_), this->dataPtr->executor_, controllerManagerNodeName, - this->dataPtr->robot_namespace_)); + this->dataPtr->node_->get_namespace())); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) { From 58579886952e5a85f8b42b29a36e0d4973ad417d Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Mon, 9 Jan 2023 18:22:59 +0100 Subject: [PATCH 13/14] remove controllerManagerPrefixNodeName logic --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index f8190d8f..574e3100 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -285,12 +285,6 @@ void IgnitionROS2ControlPlugin::Configure( // Get controller manager node name std::string controllerManagerNodeName{"controller_manager"}; - std::string controllerManagerPrefixNodeName = - _sdf->Get("controller_manager_prefix_node_name"); - if (!controllerManagerPrefixNodeName.empty()) { - controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; - } - std::string ns = "/"; if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); @@ -332,9 +326,6 @@ void IgnitionROS2ControlPlugin::Configure( } std::string node_name = "ignition_ros_control"; - if (!controllerManagerPrefixNodeName.empty()) { - node_name = controllerManagerPrefixNodeName + "_" + node_name; - } this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); this->dataPtr->executor_ = std::make_shared(); From 45bd00008b29a1f9186775d1a2fcf6ddcc2225ff Mon Sep 17 00:00:00 2001 From: "hugo.borne" Date: Mon, 13 Feb 2023 13:39:23 +0100 Subject: [PATCH 14/14] reanming node to gz_ros2 --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 574e3100..60f25a0c 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -325,7 +325,7 @@ void IgnitionROS2ControlPlugin::Configure( rclcpp::init(static_cast(argv.size()), argv.data()); } - std::string node_name = "ignition_ros_control"; + std::string node_name = "gz_ros2_control"; this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); this->dataPtr->executor_ = std::make_shared();