[JTC] Convert lambda to class functions (#945)

* Convert lambdas to member functions

* Make member function const

* Add a test for compute_error function

* Make reference_wrapper argument const

* Iterate over error fields

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -209,6 +209,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void goal_accepted_callback(
     std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
 
+  using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
+
+  /**
+   * Computes the error for a specific joint in the trajectory.
+   *
+   * @param[out] error The computed error for the joint.
+   * @param[in] index The index of the joint in the trajectory.
+   * @param[in] current The current state of the joints.
+   * @param[in] desired The desired state of the joints.
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  void compute_error_for_joint(
+    JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
+    const JointTrajectoryPoint & desired) const;
   // fill trajectory_msg so it matches joints controlled by this controller
   // positions set to current position, velocities, accelerations and efforts to 0.0
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
@@ -217,7 +231,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // sorts the joints of the incoming message to our local order
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void sort_to_local_joint_order(
-    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg);
+    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
@@ -251,7 +265,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool has_active_trajectory() const;
 
-  using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void publish_state(
     const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
@@ -283,6 +296,24 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
+
+  /**
+   * @brief Assigns the values from a trajectory point interface to a joint interface.
+   *
+   * @tparam T The type of the joint interface.
+   * @param[out] joint_interface The reference_wrapper to assign the values to
+   * @param[in] trajectory_point_interface Containing the values to assign.
+   * @todo: Use auto in parameter declaration with c++20
+   */
+  template <typename T>
+  void assign_interface_from_point(
+    const T & joint_interface, const std::vector<double> & trajectory_point_interface)
+  {
+    for (size_t index = 0; index < dof_; ++index)
+    {
+      joint_interface[index].get().set_value(trajectory_point_interface[index]);
+    }
+  }
 };
 
 }  // namespace joint_trajectory_controller

joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

@@ -147,7 +147,7 @@ inline bool check_state_tolerance_per_joint(
   if (show_errors)
   {
     const auto logger = rclcpp::get_logger("tolerances");
-    RCLCPP_ERROR(logger, "State tolerances failed for joint %ld:", joint_idx);
+    RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx);
 
     if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position)
     {

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -122,35 +122,6 @@ controller_interface::return_type JointTrajectoryController::update(
     }
   }
 
-  auto compute_error_for_joint = [&](
-                                   JointTrajectoryPoint & error, size_t index,
-                                   const JointTrajectoryPoint & current,
-                                   const JointTrajectoryPoint & desired)
-  {
-    // error defined as the difference between current and desired
-    if (joints_angle_wraparound_[index])
-    {
-      // if desired, the shortest_angular_distance is calculated, i.e., the error is
-      //  normalized between -pi<error<pi
-      error.positions[index] =
-        angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
-    }
-    else
-    {
-      error.positions[index] = desired.positions[index] - current.positions[index];
-    }
-    if (
-      has_velocity_state_interface_ &&
-      (has_velocity_command_interface_ || has_effort_command_interface_))
-    {
-      error.velocities[index] = desired.velocities[index] - current.velocities[index];
-    }
-    if (has_acceleration_state_interface_ && has_acceleration_command_interface_)
-    {
-      error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
-    }
-  };
-
   // don't update goal after we sampled the trajectory to avoid any racecondition
   const auto active_goal = *rt_active_goal_.readFromRT();
 
@@ -168,17 +139,6 @@ controller_interface::return_type JointTrajectoryController::update(
     traj_external_point_ptr_->update(*new_external_msg);
   }
 
-  // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not
-  // changed, but its value only?
-  auto assign_interface_from_point =
-    [&](auto & joint_interface, const std::vector<double> & trajectory_point_interface)
-  {
-    for (size_t index = 0; index < dof_; ++index)
-    {
-      joint_interface[index].get().set_value(trajectory_point_interface[index]);
-    }
-  };
-
   // current state update
   state_current_.time_from_start.set__sec(0);
   read_state_from_state_interfaces(state_current_);
@@ -1191,6 +1151,34 @@ void JointTrajectoryController::goal_accepted_callback(
     std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
 }
 
+void JointTrajectoryController::compute_error_for_joint(
+  JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
+  const JointTrajectoryPoint & desired) const
+{
+  // error defined as the difference between current and desired
+  if (joints_angle_wraparound_[index])
+  {
+    // if desired, the shortest_angular_distance is calculated, i.e., the error is
+    //  normalized between -pi<error<pi
+    error.positions[index] =
+      angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
+  }
+  else
+  {
+    error.positions[index] = desired.positions[index] - current.positions[index];
+  }
+  if (
+    has_velocity_state_interface_ &&
+    (has_velocity_command_interface_ || has_effort_command_interface_))
+  {
+    error.velocities[index] = desired.velocities[index] - current.velocities[index];
+  }
+  if (has_acceleration_state_interface_ && has_acceleration_command_interface_)
+  {
+    error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
+  }
+}
+
 void JointTrajectoryController::fill_partial_goal(
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const
 {
@@ -1252,7 +1240,7 @@ void JointTrajectoryController::fill_partial_goal(
 }
 
 void JointTrajectoryController::sort_to_local_joint_order(
-  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg)
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const
 {
   // rearrange all points in the trajectory message based on mapping
   std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params_.joints);

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -461,6 +461,180 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
 
 // Floating-point value comparison threshold
 const double EPS = 1e-6;
+
+/**
+ * @brief check if calculated trajectory error is correct with angle wraparound=true
+ */
+TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  std::vector<rclcpp::Parameter> params = {};
+  SetUpAndActivateTrajectoryController(
+    executor, params, true, 0.0, 1.0, true /* angle_wraparound */);
+
+  size_t n_joints = joint_names_.size();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  std::vector<std::vector<double>> points_accelerations{
+    {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}};
+  // *INDENT-ON*
+
+  trajectory_msgs::msg::JointTrajectoryPoint error, current, desired;
+  current.positions = {points[0].begin(), points[0].end()};
+  current.velocities = {points_velocities[0].begin(), points_velocities[0].end()};
+  current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()};
+  traj_controller_->resize_joint_trajectory_point(error, n_joints);
+
+  // zero error
+  desired = current;
+  for (size_t i = 0; i < n_joints; ++i)
+  {
+    traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
+    EXPECT_NEAR(error.positions[i], 0., EPS);
+    if (
+      traj_controller_->has_velocity_state_interface() &&
+      (traj_controller_->has_velocity_command_interface() ||
+       traj_controller_->has_effort_command_interface()))
+    {
+      // expect: error.velocities = desired.velocities - current.velocities;
+      EXPECT_NEAR(error.velocities[i], 0., EPS);
+    }
+    if (
+      traj_controller_->has_acceleration_state_interface() &&
+      traj_controller_->has_acceleration_command_interface())
+    {
+      // expect: error.accelerations = desired.accelerations - current.accelerations;
+      EXPECT_NEAR(error.accelerations[i], 0., EPS);
+    }
+  }
+
+  // angle wraparound of position error
+  desired.positions[0] += 3.0 * M_PI_2;
+  desired.velocities[0] += 1.0;
+  desired.accelerations[0] += 1.0;
+  traj_controller_->resize_joint_trajectory_point(error, n_joints);
+  for (size_t i = 0; i < n_joints; ++i)
+  {
+    traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
+    if (i == 0)
+    {
+      EXPECT_NEAR(
+        error.positions[i], desired.positions[i] - current.positions[i] - 2.0 * M_PI, EPS);
+    }
+    else
+    {
+      EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS);
+    }
+
+    if (
+      traj_controller_->has_velocity_state_interface() &&
+      (traj_controller_->has_velocity_command_interface() ||
+       traj_controller_->has_effort_command_interface()))
+    {
+      // expect: error.velocities = desired.velocities - current.velocities;
+      EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS);
+    }
+    if (
+      traj_controller_->has_acceleration_state_interface() &&
+      traj_controller_->has_acceleration_command_interface())
+    {
+      // expect: error.accelerations = desired.accelerations - current.accelerations;
+      EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS);
+    }
+  }
+
+  executor.cancel();
+}
+
+/**
+ * @brief check if calculated trajectory error is correct with angle wraparound=false
+ */
+TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  std::vector<rclcpp::Parameter> params = {};
+  SetUpAndActivateTrajectoryController(
+    executor, params, true, 0.0, 1.0, false /* angle_wraparound */);
+
+  size_t n_joints = joint_names_.size();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  std::vector<std::vector<double>> points_accelerations{
+    {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}};
+  // *INDENT-ON*
+
+  trajectory_msgs::msg::JointTrajectoryPoint error, current, desired;
+  current.positions = {points[0].begin(), points[0].end()};
+  current.velocities = {points_velocities[0].begin(), points_velocities[0].end()};
+  current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()};
+  traj_controller_->resize_joint_trajectory_point(error, n_joints);
+
+  // zero error
+  desired = current;
+  for (size_t i = 0; i < n_joints; ++i)
+  {
+    traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
+    EXPECT_NEAR(error.positions[i], 0., EPS);
+    if (
+      traj_controller_->has_velocity_state_interface() &&
+      (traj_controller_->has_velocity_command_interface() ||
+       traj_controller_->has_effort_command_interface()))
+    {
+      // expect: error.velocities = desired.velocities - current.velocities;
+      EXPECT_NEAR(error.velocities[i], 0., EPS);
+    }
+    if (
+      traj_controller_->has_acceleration_state_interface() &&
+      traj_controller_->has_acceleration_command_interface())
+    {
+      // expect: error.accelerations = desired.accelerations - current.accelerations;
+      EXPECT_NEAR(error.accelerations[i], 0., EPS);
+    }
+  }
+
+  // no normalization of position error
+  desired.positions[0] += 3.0 * M_PI_4;
+  desired.velocities[0] += 1.0;
+  desired.accelerations[0] += 1.0;
+  traj_controller_->resize_joint_trajectory_point(error, n_joints);
+  for (size_t i = 0; i < n_joints; ++i)
+  {
+    traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
+    EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS);
+    if (
+      traj_controller_->has_velocity_state_interface() &&
+      (traj_controller_->has_velocity_command_interface() ||
+       traj_controller_->has_effort_command_interface()))
+    {
+      // expect: error.velocities = desired.velocities - current.velocities;
+      EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS);
+    }
+    if (
+      traj_controller_->has_acceleration_state_interface() &&
+      traj_controller_->has_acceleration_command_interface())
+    {
+      // expect: error.accelerations = desired.accelerations - current.accelerations;
+      EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS);
+    }
+  }
+
+  executor.cancel();
+}
+
 /**
  * @brief check if position error of revolute joints are angle_wraparound if not configured so
  */

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -107,6 +107,13 @@ class TestableJointTrajectoryController
 
   void trigger_declare_parameters() { param_listener_->declare_params(); }
 
+  void testable_compute_error_for_joint(
+    JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
+    const JointTrajectoryPoint & desired)
+  {
+    compute_error_for_joint(error, index, current, desired);
+  }
+
   trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const
   {
     return last_commanded_state_;
@@ -154,6 +161,23 @@ class TestableJointTrajectoryController
   trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
   trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
 
+  /**
+   * a copy of the private member function
+   */
+  void resize_joint_trajectory_point(
+    trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
+  {
+    point.positions.resize(size, 0.0);
+    if (has_velocity_state_interface_)
+    {
+      point.velocities.resize(size, 0.0);
+    }
+    if (has_acceleration_state_interface_)
+    {
+      point.accelerations.resize(size, 0.0);
+    }
+  }
+
   rclcpp::WaitSet joint_cmd_sub_wait_set_;
 };