diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index bb896fc9b7..0866aab8b8 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -73,6 +73,16 @@ allow_nonzero_velocity_at_trajectory_end (boolean) Default: true +cmd_timeout (double) + Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + ``cmd_timeout`` must be greater than ``constraints.goal_time``, + otherwise ignored. + + If zero, timeout is deactivated" + + Default: 0.0 + 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/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 5a2236f437..522164776e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -173,7 +173,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; - realtime_tools::RealtimeBuffer rt_is_holding_; ///< are we holding position? + // Timeout to consider commands old + double cmd_timeout_; + // Are we holding position? + realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d2d0dc9dbd..3bd4873a31 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -139,9 +139,6 @@ class Trajectory return trajectory_msg_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e935d27a66..3a25184e24 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -189,7 +189,7 @@ controller_interface::return_type JointTrajectoryController::update( state_current_.time_from_start.set__sec(0); read_state_from_hardware(state_current_); - // currently carrying out a trajectory. + // currently carrying out a trajectory if (has_active_trajectory()) { bool first_sample = false; @@ -214,12 +214,32 @@ controller_interface::return_type JointTrajectoryController::update( if (valid_point) { + const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + // this is the time instance + // - started with the first segment: when the first point will be reached (in the future) + // - later: when the point of the current segment was reached + const rclcpp::Time segment_time_from_start = traj_start + start_segment_itr->time_from_start; + // time_difference is + // - negative until first point is reached + // - counting from zero to time_from_start of next point + double time_difference = time.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - double time_difference = 0.0; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + // have we reached the end, are not holding position, and is a timeout configured? + // Check independently of other tolerances + if ( + !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && + time_difference > cmd_timeout_) + { + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) { @@ -246,12 +266,6 @@ controller_interface::return_type JointTrajectoryController::update( if (default_tolerances_.goal_time_tolerance != 0.0) { - // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - time_difference = time.seconds() - traj_end.seconds(); - if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; @@ -946,6 +960,26 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 00d4517308..650175bae6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -91,6 +91,14 @@ joint_trajectory_controller: default_value: true, description: "If false, the last velocity point has to be zero or the goal will be rejected", } + cmd_timeout: { + type: double, + default_value: 0.0, # seconds + description: "Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + cmd_timeout must be greater than constraints.goal_time, otherwise ignored. + If zero, timeout is deactivated", + } gains: __map_joints: p: { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 69d14efa5f..7af355eae9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -617,6 +617,147 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) executor.cancel(); } +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, accept_correct_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + double cmd_timeout = 3.0; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(cmd_timeout, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 1.0); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(0.0, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief check if no timeout is triggered + */ +TEST_P(TrajectoryControllerTestParameterized, no_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); + subscribeToState(); + + 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> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + + // is the trajectory still active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should still hold the points from above + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + // value of velocities is different from above due to spline interpolation + EXPECT_GT(state_msg->reference.velocities[0], 0.0); + EXPECT_GT(state_msg->reference.velocities[1], 0.0); + EXPECT_GT(state_msg->reference.velocities[2], 0.0); + + executor.cancel(); +} + +/** + * @brief check if timeout is triggered + */ +TEST_P(TrajectoryControllerTestParameterized, timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double cmd_timeout = 0.1; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); + subscribeToState(); + + // 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> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // update until end of trajectory -> no timeout should have occurred + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3); + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should have the trajectory with three points + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + + // update until timeout should have happened + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // after timeout, set_hold_position adds a new trajectory + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should be not more than one point now (from hold position) + EXPECT_FALSE(traj_controller_->has_nontrivial_traj()); + // should hold last position with zero velocity + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(points.at(2)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are normalized if configured so */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index d222e90e9c..6bccff9a6b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -137,6 +137,13 @@ class TestableJointTrajectoryController return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; } + bool has_nontrivial_traj() + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + } + + double get_cmd_timeout() { return cmd_timeout_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; };