[JSB] added fixes to mantain the joint names order

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;
 }
 
@@ -208,23 +219,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();
@@ -234,52 +228,52 @@ 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());
 
   // 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);
     }
   }