[JSB] added fixes to mantain the joint names order

joint_state_broadcaster/doc/userdoc.rst

@@ -38,3 +38,25 @@ 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 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 and independent of the order in the `ros2_control` tag.
+
+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, 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.

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;
 }
 
@@ -199,23 +210,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate(
   return CallbackReturn::SUCCESS;
 }
 
-template <typename T>
-bool has_any_key(
-  const std::unordered_map<std::string, T> & map, const std::vector<std::string> & 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();
@@ -226,52 +220,67 @@ bool JointStateBroadcaster::init_joint_data()
   }
 
   // loop in reverse order, this maintains the order of values at retrieval time
+  const std::vector<std::string> 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();
     if (map_interface_to_joint_state_.count(interface_name) > 0)
     {
       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);
+        if (std::find(joint_names_.begin(), joint_names_.end(), prefix_name) == joint_names_.end())
+        {
+          joint_names_.push_back(prefix_name);
+        }
       }
     }
   }
+  std::reverse(joint_names_.begin(), joint_names_.end());
+  if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty())
+  {
+    std::vector<std::string> 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
-  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<std::string> & 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);
     }
   }