[backport] Fix #730 (#736)

* Fix file name for include guard (backport #681) (cherry picked from commit c619aac) Co-authored-by: Christoph Fröhlich <[email protected]> * 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) * Enable effort rejection test --------- Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>
ros-controls · Aug 5, 2023 · 6adb847 · 6adb847
1 parent 17e14e3
commit 6adb847
Show file tree

Hide file tree

Showing 15 changed files with 552 additions and 1,050 deletions.
diff --git a/.github/workflows/rolling-binary-build-main.yml b/.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

diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.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

diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.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

diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.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

diff --git a/admittance_controller/test/test_admittance_controller.cpp b/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)
 {

diff --git a/admittance_controller/test/test_admittance_controller.hpp b/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);
   }
 };
 

diff --git a/..._controller/test/6d_robot_description.hpp → .../test/test_asset_6d_robot_description.hpp b/..._controller/test/6d_robot_description.hpp → .../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_
diff --git a/joint_trajectory_controller/CMakeLists.txt b/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()
 
 

diff --git a/joint_trajectory_controller/doc/userdoc.rst b/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.
 

diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/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_
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/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)
             {
@@ -1410,6 +1421,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)
   {

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/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: {