Merge branch 'humble' into mergify/bp/humble/pr-705

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -28,6 +28,9 @@
 
 namespace admittance_controller
 {
+
+constexpr auto NUM_CARTESIAN_DOF = 6;  // (3 translation + 3 rotation)
+
 /// Configure admittance rule memory for num joints and load kinematics interface
 controller_interface::return_type AdmittanceRule::configure(
   const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
@@ -83,10 +86,10 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
   {
     state_message_.joint_state.name = parameters_.joints;
   }
-  state_message_.mass.data.resize(6, 0.0);
-  state_message_.selected_axes.data.resize(6, 0);
-  state_message_.damping.data.resize(6, 0);
-  state_message_.stiffness.data.resize(6, 0);
+  state_message_.mass.data.resize(NUM_CARTESIAN_DOF, 0.0);
+  state_message_.selected_axes.data.resize(NUM_CARTESIAN_DOF, 0);
+  state_message_.damping.data.resize(NUM_CARTESIAN_DOF, 0);
+  state_message_.stiffness.data.resize(NUM_CARTESIAN_DOF, 0);
   state_message_.wrench_base.header.frame_id = parameters_.kinematics.base;
   state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base;
   state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base;
@@ -122,7 +125,7 @@ void AdmittanceRule::apply_parameters_update()
   vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness);
   vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes);
 
-  for (size_t i = 0; i < 6; ++i)
+  for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i)
   {
     admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i];
     admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 *
@@ -331,16 +334,20 @@ void AdmittanceRule::process_wrench_measurements(
 
 const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state()
 {
+  for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i)
+  {
+    state_message_.stiffness.data[i] = admittance_state_.stiffness[i];
+    state_message_.damping.data[i] = admittance_state_.damping[i];
+    state_message_.selected_axes.data[i] = static_cast<bool>(admittance_state_.selected_axes[i]);
+    state_message_.mass.data[i] = admittance_state_.mass[i];
+  }
+
   for (size_t i = 0; i < parameters_.joints.size(); ++i)
   {
     state_message_.joint_state.name[i] = parameters_.joints[i];
     state_message_.joint_state.position[i] = admittance_state_.joint_pos[i];
     state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i];
     state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i];
-    state_message_.stiffness.data[i] = admittance_state_.stiffness[i];
-    state_message_.damping.data[i] = admittance_state_.damping[i];
-    state_message_.selected_axes.data[i] = static_cast<bool>(admittance_state_.selected_axes[i]);
-    state_message_.mass.data[i] = admittance_state_.mass[i];
   }
 
   state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0];

admittance_controller/test/test_admittance_controller.hpp

@@ -27,7 +27,6 @@
 
 #include "gmock/gmock.h"
 
-#include "6d_robot_description.hpp"
 #include "admittance_controller/admittance_controller.hpp"
 #include "control_msgs/msg/admittance_controller_state.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -38,6 +37,7 @@
 #include "rclcpp/utilities.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
+#include "test_asset_6d_robot_description.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 

admittance_controller/test/6d_robot_description.hpp → admittance_controller/test/test_asset_6d_robot_description.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_
-#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_
+#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_
+#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_
 
 #include <string>
 
@@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf =
 
 }  // namespace ros2_control_test_assets
 
-#endif  // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_
+#endif  // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_

doc/writing_new_controller.rst

@@ -138,4 +138,4 @@ That's it! Enjoy writing great controllers!
 Useful External References
 ---------------------------
 
-- `Templates and scripts for generating controllers shell <https://stoglrobotics.github.io/ros_team_workspace/use-cases/ros2_control/setup_controller.html>`_
+- `Templates and scripts for generating controllers shell <https://rtw.stoglrobotics.de/master/use-cases/ros2_control/setup_controller.html>`_

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1323,8 +1323,12 @@ void JointTrajectoryController::sort_to_local_joint_order(
         get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size());
       return to_remap;
     }
-    std::vector<double> output;
-    output.resize(mapping.size(), 0.0);
+    static std::vector<double> output(dof_, 0.0);
+    // Only resize if necessary since it's an expensive operation
+    if (output.size() != mapping.size())
+    {
+      output.resize(mapping.size(), 0.0);
+    }
     for (size_t index = 0; index < mapping.size(); ++index)
     {
       auto map_index = mapping[index];