From 16700aa251bf65fdf1520462d08ed638ba97302b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Mar 2025 23:50:51 +0100 Subject: [PATCH 01/13] [JSB] added fixes to mantain the joint names order --- .../src/joint_state_broadcaster.cpp | 40 +++++++++---------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5a9d1caeef..62f4785221 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -234,12 +234,15 @@ bool JointStateBroadcaster::init_joint_data() } // loop in reverse order, this maintains the order of values at retrieval time + const std::vector joint_state_interfaces = { + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { + const std::string prefix_name = si->get_prefix_name(); // initialize map if name is new - if (name_if_value_mapping_.count(si->get_prefix_name()) == 0) + if (name_if_value_mapping_.count(prefix_name) == 0) { - name_if_value_mapping_[si->get_prefix_name()] = {}; + name_if_value_mapping_[prefix_name] = {}; } // add interface name std::string interface_name = si->get_interface_name(); @@ -247,39 +250,32 @@ bool JointStateBroadcaster::init_joint_data() { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; - } + name_if_value_mapping_[prefix_name][interface_name] = kUninitializedValue; - // filter state interfaces that have at least one of the joint_states fields, - // the rest will be ignored for this message - for (const auto & name_ifv : name_if_value_mapping_) - { - const auto & interfaces_and_values = name_ifv.second; - if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) + // filter state interfaces that have at least one of the joint_states fields, + // the rest will be ignored for this message + if ( + std::find(joint_state_interfaces.begin(), joint_state_interfaces.end(), interface_name) != + joint_state_interfaces.end()) { if ( !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || - model_.getJoint(name_ifv.first)) + model_.getJoint(prefix_name)) { - joint_names_.push_back(name_ifv.first); + joint_names_.push_back(prefix_name); } } } // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there - rclcpp::Parameter extra_joints; - if (get_node()->get_parameter("extra_joints", extra_joints)) + for (const auto & extra_joint_name : params_.extra_joints) { - const std::vector & extra_joints_names = extra_joints.as_string_array(); - for (const auto & extra_joint_name : extra_joints_names) + if (name_if_value_mapping_.count(extra_joint_name) == 0) { - if (name_if_value_mapping_.count(extra_joint_name) == 0) - { - name_if_value_mapping_[extra_joint_name] = { - {HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}}; - joint_names_.push_back(extra_joint_name); - } + name_if_value_mapping_[extra_joint_name] = { + {HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}}; + joint_names_.push_back(extra_joint_name); } } From f1a4f142e668572dd9f6b3e919cc138da7657a0a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Mar 2025 23:52:21 +0100 Subject: [PATCH 02/13] remove has_any_key method --- .../src/joint_state_broadcaster.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 62f4785221..47d624d446 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -208,23 +208,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( return CallbackReturn::SUCCESS; } -template -bool has_any_key( - const std::unordered_map & map, const std::vector & keys) -{ - bool found_key = false; - for (const auto & key_item : map) - { - const auto & key = key_item.first; - if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend()) - { - found_key = true; - break; - } - } - return found_key; -} - bool JointStateBroadcaster::init_joint_data() { joint_names_.clear(); From f1d499f9c196690a9d1a61cf25dc6b6039c12b54 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Mar 2025 00:05:17 +0100 Subject: [PATCH 03/13] Reserve some memory in the configure method --- .../src/joint_state_broadcaster.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 47d624d446..d54fb6ce44 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -170,6 +170,17 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); } + // joint_names reserve space for all joints + const auto max_joints_size = + (params_.joints.empty() ? model_.joints_.size() : params_.joints.size()) + + params_.extra_joints.size(); + joint_names_.reserve(max_joints_size); + auto & joint_state_msg = realtime_joint_state_publisher_->msg_; + joint_state_msg.name.reserve(max_joints_size); + joint_state_msg.position.reserve(max_joints_size); + joint_state_msg.velocity.reserve(max_joints_size); + joint_state_msg.effort.reserve(max_joints_size); + return CallbackReturn::SUCCESS; } From b123d21019a135f77a995513cec83671bb460a8a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Mar 2025 00:32:30 +0100 Subject: [PATCH 04/13] filter duplicates --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index d54fb6ce44..969e376026 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -256,7 +256,10 @@ bool JointStateBroadcaster::init_joint_data() !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || model_.getJoint(prefix_name)) { - joint_names_.push_back(prefix_name); + if (std::find(joint_names_.begin(), joint_names_.end(), prefix_name) == joint_names_.end()) + { + joint_names_.push_back(prefix_name); + } } } } From 815b37a58fc097c65b7f0157876935f41cac48d6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Mar 2025 00:39:29 +0100 Subject: [PATCH 05/13] Reverse the order as we are iterating from back --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 969e376026..c4b32a6aff 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -263,6 +263,7 @@ bool JointStateBroadcaster::init_joint_data() } } } + std::reverse(joint_names_.begin(), joint_names_.end()); // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there From 16e891f2c15996e97354f40bb504771dce6ba5fe Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Mar 2025 11:38:24 +0100 Subject: [PATCH 06/13] Use URDF Model to organize the joints --- .../src/joint_state_broadcaster.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c4b32a6aff..5e4d9eba6b 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -264,6 +264,21 @@ bool JointStateBroadcaster::init_joint_data() } } std::reverse(joint_names_.begin(), joint_names_.end()); + if (params_.use_urdf_to_filter) + { + std::vector joint_names_filtered; + for (const auto & [joint_name, urdf_joint] : model_.joints_) + { + if (urdf_joint && urdf_joint->type != urdf::Joint::FIXED) + { + if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) != joint_names_.end()) + { + joint_names_filtered.push_back(joint_name); + } + } + } + joint_names_ = joint_names_filtered; + } // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there From 5acf78b24eb131aad88076f53bcb8fc7e32e704d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Mar 2025 11:55:50 +0100 Subject: [PATCH 07/13] Revert "Use URDF Model to organize the joints" This reverts commit 16e891f2c15996e97354f40bb504771dce6ba5fe. --- .../src/joint_state_broadcaster.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5e4d9eba6b..c4b32a6aff 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -264,21 +264,6 @@ bool JointStateBroadcaster::init_joint_data() } } std::reverse(joint_names_.begin(), joint_names_.end()); - if (params_.use_urdf_to_filter) - { - std::vector joint_names_filtered; - for (const auto & [joint_name, urdf_joint] : model_.joints_) - { - if (urdf_joint && urdf_joint->type != urdf::Joint::FIXED) - { - if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) != joint_names_.end()) - { - joint_names_filtered.push_back(joint_name); - } - } - } - joint_names_ = joint_names_filtered; - } // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there From 3475a9ce7ab2632c4a497d84bb1e0c6f305a4aa4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Mar 2025 12:10:35 +0100 Subject: [PATCH 08/13] Use URDF Model to organize the joints again This reverts commit 5acf78b24eb131aad88076f53bcb8fc7e32e704d. --- .../src/joint_state_broadcaster.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c4b32a6aff..eaaf8aa10d 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -264,6 +264,21 @@ bool JointStateBroadcaster::init_joint_data() } } std::reverse(joint_names_.begin(), joint_names_.end()); + if (is_model_loaded_ && params_.use_urdf_to_filter) + { + std::vector joint_names_filtered; + for (const auto & [joint_name, urdf_joint] : model_.joints_) + { + if (urdf_joint && urdf_joint->type != urdf::Joint::FIXED) + { + if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) != joint_names_.end()) + { + joint_names_filtered.push_back(joint_name); + } + } + } + joint_names_ = joint_names_filtered; + } // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there From 25d352920a585ad4d0dee971bcd59185aa51b0d0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Mar 2025 23:37:45 +0100 Subject: [PATCH 09/13] make sure the joints parameter is empty --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index eaaf8aa10d..aeddd30143 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -264,7 +264,7 @@ bool JointStateBroadcaster::init_joint_data() } } std::reverse(joint_names_.begin(), joint_names_.end()); - if (is_model_loaded_ && params_.use_urdf_to_filter) + if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty()) { std::vector joint_names_filtered; for (const auto & [joint_name, urdf_joint] : model_.joints_) From 391ffa314fbf5b72fb9380156f492330a6c7fac1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Mar 2025 23:56:40 +0100 Subject: [PATCH 10/13] Update docs on the order of the joint names --- joint_state_broadcaster/doc/userdoc.rst | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index c7bf4fa9a1..66c17de549 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -38,3 +38,18 @@ An example parameter file .. generate_parameter_library_default:: ../src/joint_state_broadcaster_parameters.yaml + + +Order of the joints in the message +---------------------------------- + +The order of the joints in the message can determined by 3 different parameter settings: + +1. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``false``: + The order of the joints in the message is the same as the order of the joint's state interfaces registered in the resource manager. This is typically the order in which the hardware components are loaded and configured. + +2. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``true``: + The order of the joints in the message is the same as the order of the joints in the URDF file, which is inherited from the loaded URDF Model. + +3. Defined ``joints`` parameter: + The order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. From 468ec3692fdd0636120515434d74af064e5901ab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 11 Mar 2025 00:00:48 +0100 Subject: [PATCH 11/13] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- joint_state_broadcaster/doc/userdoc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 66c17de549..7a521eada2 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -46,10 +46,10 @@ Order of the joints in the message The order of the joints in the message can determined by 3 different parameter settings: 1. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``false``: - The order of the joints in the message is the same as the order of the joint's state interfaces registered in the resource manager. This is typically the order in which the hardware components are loaded and configured. + The order of the joints in the message is the same as the order of the joints' state interfaces registered in the resource manager. This is typically the order in which the hardware components are loaded and configured. 2. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``true``: - The order of the joints in the message is the same as the order of the joints in the URDF file, which is inherited from the loaded URDF Model. + The order of the joints in the message is the same as the order of the joints in the URDF file, which is inherited from the loaded URDF model and independent of the order in the `ros2_control` tag. 3. Defined ``joints`` parameter: The order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. From 99d003962ee9da46b49fe6f1af97be7fc018976c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Mar 2025 23:56:59 +0100 Subject: [PATCH 12/13] Update documentation --- joint_state_broadcaster/doc/userdoc.rst | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 7a521eada2..8bed184f37 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -51,5 +51,9 @@ The order of the joints in the message can determined by 3 different parameter s 2. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``true``: The order of the joints in the message is the same as the order of the joints in the URDF file, which is inherited from the loaded URDF model and independent of the order in the `ros2_control` tag. -3. Defined ``joints`` parameter: +3. Defined ``joints`` parameter along with ``interfaces`` parameter: The order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. + + If the ``joints`` parameter is a subset of the total available joints in the URDF (or) the total available state interfaces, the order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. + + If any of the combinations of the defined ``joints`` parameter and ``interfaces`` parameter are not in the available state interfaces, the controller will fail to activate. From 7a0141ef874d97861633a710d83a62e478f99af6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 18 Mar 2025 19:26:30 +0100 Subject: [PATCH 13/13] Update docs --- joint_state_broadcaster/doc/userdoc.rst | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 8bed184f37..996284d51a 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -54,6 +54,9 @@ The order of the joints in the message can determined by 3 different parameter s 3. Defined ``joints`` parameter along with ``interfaces`` parameter: The order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. - If the ``joints`` parameter is a subset of the total available joints in the URDF (or) the total available state interfaces, the order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. + If the ``joints`` parameter is a subset of the total available joints in the URDF (or) the total available state interfaces, then only the joints in the ``joints`` parameter are published in the message. If any of the combinations of the defined ``joints`` parameter and ``interfaces`` parameter are not in the available state interfaces, the controller will fail to activate. + +..note:: + If the ``extra_joints`` parameter is set, the joints in the ``extra_joints`` parameter are appended to the end of the joint names in the message.