[JTC] Re-enabling test, bugfixing and hardening. Adding a parameter to define when trajectories with non-zero velocity at the end are used. (backport #705)

.github/workflows/rolling-binary-build-main.yml

@@ -6,9 +6,13 @@ on:
   workflow_dispatch:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   pull_request:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   push:
     branches:
       - master

.github/workflows/rolling-binary-build-testing.yml

@@ -6,9 +6,13 @@ on:
   workflow_dispatch:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   pull_request:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   push:
     branches:
       - master

.github/workflows/rolling-semi-binary-build-main.yml

@@ -5,9 +5,13 @@ on:
   workflow_dispatch:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   pull_request:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   push:
     branches:
       - master

.github/workflows/rolling-semi-binary-build-testing.yml

@@ -5,9 +5,13 @@ on:
   workflow_dispatch:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   pull_request:
     branches:
       - master
+      - '*feature*'
+      - '*feature/**'
   push:
     branches:
       - master

joint_trajectory_controller/CMakeLists.txt

@@ -78,13 +78,13 @@ if(BUILD_TESTING)
     ros2_control_test_assets
   )
 
-  # TODO(andyz): Disabled due to flakiness
-  # ament_add_gmock(test_trajectory_actions
-  #   test/test_trajectory_actions.cpp
-  # )
-  # target_link_libraries(test_trajectory_actions
-  #   joint_trajectory_controller
-  # )
+  ament_add_gmock(test_trajectory_actions
+    test/test_trajectory_actions.cpp
+  )
+  set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300)
+  target_link_libraries(test_trajectory_actions
+    joint_trajectory_controller
+  )
 endif()
 
 

joint_trajectory_controller/doc/userdoc.rst

@@ -159,6 +159,11 @@ open_loop_control (boolean)
 
   If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits<double>::quiet_NaN()) or state values when the hardware is started.
 
+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
+
 constraints (structure)
   Default values for tolerances if no explicit values are states in JointTrajectory message.
 

joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp

@@ -19,10 +19,11 @@
 #include <vector>
 
 #include "parameter_traits/parameter_traits.hpp"
+#include "rclcpp/parameter.hpp"
 #include "rsl/algorithm.hpp"
 #include "tl_expected/expected.hpp"
 
-namespace parameter_traits
+namespace joint_trajectory_controller
 {
 tl::expected<void, std::string> command_interface_type_combinations(
   rclcpp::Parameter const & parameter)
@@ -94,6 +95,6 @@ tl::expected<void, std::string> state_interface_type_combinations(
   return {};
 }
 
-}  // namespace parameter_traits
+}  // namespace joint_trajectory_controller
 
 #endif  // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -67,6 +67,15 @@ 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;
 }
 
@@ -136,7 +145,9 @@ controller_interface::return_type JointTrajectoryController::update(
     {
       error.positions[index] = desired.positions[index] - current.positions[index];
     }
-    if (has_velocity_state_interface_ && has_velocity_command_interface_)
+    if (
+      has_velocity_state_interface_ &&
+      (has_velocity_command_interface_ || has_effort_command_interface_))
     {
       error.velocities[index] = desired.velocities[index] - current.velocities[index];
     }
@@ -234,7 +245,7 @@ controller_interface::return_type JointTrajectoryController::update(
             const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
             const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
 
-            time_difference = get_node()->now().seconds() - traj_end.seconds();
+            time_difference = time.seconds() - traj_end.seconds();
 
             if (time_difference > default_tolerances_.goal_time_tolerance)
             {
@@ -1416,6 +1427,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
     }
   }
 
+  if (!params_.allow_nonzero_velocity_at_trajectory_end)
+  {
+    for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
+    {
+      if (trajectory.points.back().velocities.at(i) != 0.)
+      {
+        RCLCPP_ERROR(
+          get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f",
+          trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
+        return false;
+      }
+    }
+  }
+
   rclcpp::Duration previous_traj_time(0ms);
   for (size_t i = 0; i < trajectory.points.size(); ++i)
   {

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -22,7 +22,7 @@ joint_trajectory_controller:
     validation: {
       unique<>: null,
       subset_of<>: [["position", "velocity", "acceleration", "effort",]],
-      command_interface_type_combinations: null,
+      "joint_trajectory_controller::command_interface_type_combinations": null,
     }
   }
   state_interfaces: {
@@ -32,7 +32,7 @@ joint_trajectory_controller:
     validation: {
       unique<>: null,
       subset_of<>: [["position", "velocity", "acceleration",]],
-      state_interface_type_combinations: null,
+      "joint_trajectory_controller::state_interface_type_combinations": null,
     }
   }
   allow_partial_joints_goal: {
@@ -74,6 +74,11 @@ joint_trajectory_controller:
       one_of<>: [["splines", "none"]],
     }
   }
+  allow_nonzero_velocity_at_trajectory_end: {
+    type: bool,
+    default_value: true,
+    description: "If false, the last velocity point has to be zero or the goal will be rejected",
+  }
   gains:
     __map_joints:
       p: {