[backport] Fix #723 (#737)

* Small improvement in remapping (#393) (#724)

* Fix file name for include guard (backport #681)

(cherry picked from commit c619aac)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

* Activate AdmittanceControllerTestParameterizedInvalidParameters (#711) (#733)

* [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) (#706)

* Add state_publish_rate parameter

---------

Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>

.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

admittance_controller/test/test_admittance_controller.cpp

@@ -67,10 +67,18 @@ INSTANTIATE_TEST_SUITE_P(
     // wrong length selected axes
     std::make_tuple(
       std::string("admittance.selected_axes"),
-      rclcpp::ParameterValue(std::vector<double>() = {1, 2, 3})),
-    // invalid robot description
-    std::make_tuple(
-      std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))));
+      rclcpp::ParameterValue(std::vector<double>() = {1, 2, 3}))
+    // invalid robot description.
+    // TODO(anyone): deactivated, because SetUpController returns SUCCESS here?
+    // ,std::make_tuple(
+    //   std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))
+    ));
+
+// Test on_init returns ERROR when a parameter is invalid
+TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, invalid_parameters)
+{
+  ASSERT_EQ(SetUpController(), controller_interface::return_type::ERROR);
+}
 
 TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success)
 {

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"
 
@@ -454,9 +454,15 @@ class AdmittanceControllerTestParameterizedInvalidParameters
   static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); }
 
 protected:
-  void SetUpController()
+  controller_interface::return_type SetUpController()
   {
-    AdmittanceControllerTest::SetUpController("test_admittance_controller");
+    auto param_name = std::get<0>(GetParam());
+    auto param_value = std::get<1>(GetParam());
+    std::vector<rclcpp::Parameter> parameter_overrides;
+    rclcpp::Parameter param(param_name, param_value);
+    parameter_overrides.push_back(param);
+    return AdmittanceControllerTest::SetUpController(
+      "test_admittance_controller", parameter_overrides);
   }
 };
 

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_

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/parameters.rst

@@ -22,6 +22,12 @@ state_interfaces (list(string))
   Values: position (mandatory) [velocity, [acceleration]].
   Acceleration interface can only be used in combination with position and velocity.
 
+state_publish_rate (double)
+
+    Publish-rate of the controller's “state” topic.
+
+    Default: 50.0
+
 action_monitor_rate (double)
   Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory).
 

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)
             {
@@ -1312,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];
@@ -1412,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: {