[JTC] Remove deprecation warnings, set `allow_nonzero_velocity_at_tra…

…jectory_end` default false (#834)

joint_trajectory_controller/doc/parameters.rst

@@ -66,7 +66,7 @@ start_with_holding (bool)
 allow_nonzero_velocity_at_trajectory_end (boolean)
   If false, the last velocity point has to be zero or the goal will be rejected.
 
-  Default: true
+  Default: false
 
 cmd_timeout (double)
   Timeout after which the input command is considered stale.
@@ -147,5 +147,4 @@ gains.<joint_name>.angle_wraparound (bool)
   Otherwise  :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
   position :math:`s` from the state interface.
 
-
   Default: false

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -167,8 +167,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   std::vector<PidPtr> pids_;
   // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
   std::vector<double> ff_velocity_scale_;
-  // Configuration for every joint, if position error is normalized
-  std::vector<bool> normalize_joint_error_;
+  // Configuration for every joint, if position error is wrapped around
+  std::vector<bool> joints_angle_wraparound_;
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -64,15 +64,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
     return CallbackReturn::ERROR;
   }
 
-  // TODO(christophfroehlich): remove deprecation warning
-  if (params_.allow_nonzero_velocity_at_trajectory_end)
-  {
-    RCLCPP_WARN(
-      get_node()->get_logger(),
-      "[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to "
-      "true. The default behavior will change to false.");
-  }
-
   return CallbackReturn::SUCCESS;
 }
 
@@ -131,7 +122,7 @@ controller_interface::return_type JointTrajectoryController::update(
                                    const JointTrajectoryPoint & desired)
   {
     // error defined as the difference between current and desired
-    if (normalize_joint_error_[index])
+    if (joints_angle_wraparound_[index])
     {
       // if desired, the shortest_angular_distance is calculated, i.e., the error is
       //  normalized between -pi<error<pi
@@ -725,20 +716,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   }
 
   // Configure joint position error normalization from ROS parameters (angle_wraparound)
-  normalize_joint_error_.resize(dof_);
+  joints_angle_wraparound_.resize(dof_);
   for (size_t i = 0; i < dof_; ++i)
   {
     const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
-    if (gains.normalize_error)
-    {
-      // TODO(anyone): Remove deprecation warning in the end of 2023
-      RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!");
-      normalize_joint_error_[i] = gains.normalize_error;
-    }
-    else
-    {
-      normalize_joint_error_[i] = gains.angle_wraparound;
-    }
+    joints_angle_wraparound_[i] = gains.angle_wraparound;
   }
 
   if (params_.state_interfaces.empty())

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -80,7 +80,7 @@ joint_trajectory_controller:
   }
   allow_nonzero_velocity_at_trajectory_end: {
     type: bool,
-    default_value: true,
+    default_value: false,
     description: "If false, the last velocity point has to be zero or the goal will be rejected",
   }
   cmd_timeout: {
@@ -118,11 +118,6 @@ joint_trajectory_controller:
         default_value: 0.0,
         description: "Feed-forward scaling of velocity."
       }
-      normalize_error: {
-        type: bool,
-        default_value: false,
-        description: "(Deprecated) Use position error normalization to -pi to pi."
-      }
       angle_wraparound: {
         type: bool,
         default_value: false,

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -653,7 +653,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr
 
   // will be accepted despite nonzero last point
   EXPECT_TRUE(gh_future.get());
-  EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+  if ((traj_controller_->has_effort_command_interface()) == false)
+  {
+    // can't succeed with effort cmd if
+    EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+  }
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false)

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -201,8 +201,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
 TEST_P(TrajectoryControllerTestParameterized, cleanup)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  std::vector<rclcpp::Parameter> params = {};
   SetUpAndActivateTrajectoryController(executor, params);
 
   // send msg
@@ -456,14 +455,13 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
 // Floating-point value comparison threshold
 const double EPS = 1e-6;
 /**
- * @brief check if position error of revolute joints are normalized if not configured so
+ * @brief check if position error of revolute joints are angle_wraparound if not configured so
  */
-TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
+TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  std::vector<rclcpp::Parameter> params = {};
   SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
   subscribeToState();
 
@@ -706,14 +704,13 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
 }
 
 /**
- * @brief check if position error of revolute joints are normalized if configured so
+ * @brief check if position error of revolute joints are angle_wraparound if configured so
  */
-TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
+TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  std::vector<rclcpp::Parameter> params = {};
   SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true);
   subscribeToState();
 
@@ -754,7 +751,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
   EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
   EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
 
-  // is error.positions[2] normalized?
+  // is error.positions[2] angle_wraparound?
   EXPECT_NEAR(
     state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
   EXPECT_NEAR(
@@ -783,7 +780,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
         k_p * allowed_delta);
-      // is error of positions[2] normalized?
+      // is error of positions[2] angle_wraparound?
       EXPECT_GT(0.0, joint_vel_[2]);
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
@@ -811,7 +808,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
         k_p * allowed_delta);
-      // is error of positions[2] normalized?
+      // is error of positions[2] angle_wraparound?
       EXPECT_GT(0.0, joint_eff_[2]);
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -198,7 +198,7 @@ class TrajectoryControllerTest : public ::testing::Test
   }
 
   void SetPidParameters(
-    double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false)
+    double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false)
   {
     traj_controller_->trigger_declare_parameters();
     auto node = traj_controller_->get_node();
@@ -211,27 +211,31 @@ class TrajectoryControllerTest : public ::testing::Test
       const rclcpp::Parameter k_d(prefix + ".d", 0.0);
       const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0);
       const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default);
-      const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default);
-      node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error});
+      const rclcpp::Parameter angle_wraparound(
+        prefix + ".angle_wraparound", angle_wraparound_default);
+      node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound});
     }
   }
 
   void SetUpAndActivateTrajectoryController(
     rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {},
     bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
-    bool normalize_error = false)
+    bool angle_wraparound = false)
   {
     SetUpTrajectoryController(executor);
 
+    // add this to simplify tests, can be overwritten by parameters
+    rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true);
+    traj_controller_->get_node()->set_parameter(nonzero_vel_parameter);
+
     // set pid parameters before configure
-    SetPidParameters(k_p, ff, normalize_error);
+    SetPidParameters(k_p, ff, angle_wraparound);
+
+    // set optional parameters
     for (const auto & param : parameters)
     {
       traj_controller_->get_node()->set_parameter(param);
     }
-    // ignore velocity tolerances for this test since they aren't committed in test_robot->write()
-    rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0);
-    traj_controller_->get_node()->set_parameter(stopped_velocity_parameters);
 
     traj_controller_->get_node()->configure();