From fdda9384204360ff694469099ae232d420c69772 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Apr 2024 21:52:56 +0000 Subject: [PATCH 1/9] Move two methods to trajectory_operations header --- joint_trajectory_controller/CMakeLists.txt | 4 +- .../joint_trajectory_controller.hpp | 7 +- .../trajectory_operations.hpp | 204 +++++++++++++++ .../src/joint_trajectory_controller.cpp | 156 +----------- .../test/test_trajectory_controller.cpp | 163 ------------ .../test/test_trajectory_controller_utils.hpp | 1 - .../test/test_trajectory_operations.cpp | 234 ++++++++++++++++++ 7 files changed, 445 insertions(+), 324 deletions(-) create mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp create mode 100644 joint_trajectory_controller/test/test_trajectory_operations.cpp diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 4b0ca82df8..7fc2a2a8d8 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -59,7 +59,9 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_link_libraries(test_trajectory joint_trajectory_controller) - target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + + ament_add_gmock(test_trajectory_operations test/test_trajectory_operations.cpp) + target_link_libraries(test_trajectory_operations joint_trajectory_controller) ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) 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 ae370df0a6..ff6912d51c 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 @@ -30,6 +30,7 @@ #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" +#include "joint_trajectory_controller/trajectory_operations.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" @@ -235,14 +236,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void sort_to_local_joint_order( std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void add_new_trajectory_msg( const std::shared_ptr & traj_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const; SegmentTolerances default_tolerances_; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp new file mode 100644 index 0000000000..9d167bb581 --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp @@ -0,0 +1,204 @@ +// Copyright (c) 2024 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_OPERATIONS_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_OPERATIONS_HPP_ + +#include +#include +#include +#include +#include + +#include "joint_trajectory_controller/visibility_control.h" +#include "rclcpp/logging.hpp" +#include "rclcpp/time.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +// auto-generated by generate_parameter_library +#include "joint_trajectory_controller_parameters.hpp" + +namespace joint_trajectory_controller +{ +using namespace std::chrono_literals; // NOLINT + +// // fill trajectory_msg so it matches joints controlled by this controller +// // positions set to current position, velocities, accelerations and efforts to 0.0 +// JOINT_TRAJECTORY_CONTROLLER_PUBLIC +// void fill_partial_goal( +// std::shared_ptr trajectory_msg) const; +// // sorts the joints of the incoming message to our local order +// JOINT_TRAJECTORY_CONTROLLER_PUBLIC +// void sort_to_local_joint_order( +// std::shared_ptr trajectory_msg) const; + +JOINT_TRAJECTORY_CONTROLLER_PUBLIC +bool validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, const bool allow_empty, + rclcpp::Logger logger) +{ + if (allow_empty && vector_field.empty()) + { + return true; + } + if (joint_names_size != vector_field.size()) + { + RCLCPP_ERROR( + logger, "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", + joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + return false; + } + return true; +} + +JOINT_TRAJECTORY_CONTROLLER_PUBLIC +bool validate_trajectory_msg( + const trajectory_msgs::msg::JointTrajectory & trajectory, rclcpp::Logger logger, rclcpp::Time now, + const Params params) +{ + // If partial joints goals are not allowed, goal should specify all controller joints + if (!params.allow_partial_joints_goal) + { + if (trajectory.joint_names.size() != params.joints.size()) + { + RCLCPP_ERROR(logger, "Joints on incoming trajectory don't match the controller joints."); + return false; + } + } + + if (trajectory.joint_names.empty()) + { + RCLCPP_ERROR(logger, "Empty joint names on incoming trajectory."); + return false; + } + + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) + { + trajectory_end_time += p.time_from_start; + } + if (trajectory_end_time < now) + { + RCLCPP_ERROR( + logger, "Received trajectory with non-zero start time (%f) that ends in the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + + for (size_t i = 0; i < trajectory.joint_names.size(); ++i) + { + const std::string & incoming_joint_name = trajectory.joint_names[i]; + + auto it = std::find(params.joints.begin(), params.joints.end(), incoming_joint_name); + if (it == params.joints.end()) + { + RCLCPP_ERROR( + logger, "Incoming joint %s doesn't match the controller's joints.", + incoming_joint_name.c_str()); + return false; + } + } + + if (trajectory.points.empty()) + { + RCLCPP_ERROR(logger, "Empty trajectory received."); + return false; + } + + if (!params.allow_nonzero_velocity_at_trajectory_end) + { + for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) + { + if ( + std::fabs(trajectory.points.back().velocities.at(i)) > + std::numeric_limits::epsilon()) + { + RCLCPP_ERROR( + logger, "Velocity of last trajectory point of joint %s is not zero: %.15f", + 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) + { + if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) + { + RCLCPP_ERROR( + logger, + "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", + i - 1, i, previous_traj_time.seconds(), + rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); + return false; + } + previous_traj_time = trajectory.points[i].time_from_start; + + const size_t joint_count = trajectory.joint_names.size(); + const auto & points = trajectory.points; + // This currently supports only position, velocity and acceleration inputs + if (params.allow_integration_in_goal_trajectories) + { + const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty(); + const bool position_error = + !points[i].positions.empty() && + !validate_trajectory_point_field( + joint_count, points[i].positions, "positions", i, false, logger); + const bool velocity_error = + !points[i].velocities.empty() && + !validate_trajectory_point_field( + joint_count, points[i].velocities, "velocities", i, false, logger); + const bool acceleration_error = + !points[i].accelerations.empty() && + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, false, logger); + if (all_empty || position_error || velocity_error || acceleration_error) + { + return false; + } + } + else if ( + !validate_trajectory_point_field( + joint_count, points[i].positions, "positions", i, false, logger) || + !validate_trajectory_point_field( + joint_count, points[i].velocities, "velocities", i, true, logger) || + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, true, logger)) + { + return false; + } + // reject effort entries + if (!points[i].effort.empty()) + { + RCLCPP_ERROR(logger, "Trajectories with effort fields are currently not supported."); + return false; + } + } + return true; +} + +} // namespace joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_OPERATIONS_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a4b63cb75..ad6ff25bf9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1103,7 +1103,7 @@ void JointTrajectoryController::publish_state( void JointTrajectoryController::topic_callback( const std::shared_ptr msg) { - if (!validate_trajectory_msg(*msg)) + if (!validate_trajectory_msg(*msg, get_node()->get_logger(), get_node()->now(), params_)) { return; } @@ -1129,7 +1129,8 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( return rclcpp_action::GoalResponse::REJECT; } - if (!validate_trajectory_msg(goal->trajectory)) + if (!validate_trajectory_msg( + goal->trajectory, get_node()->get_logger(), get_node()->now(), params_)) { return rclcpp_action::GoalResponse::REJECT; } @@ -1330,157 +1331,6 @@ void JointTrajectoryController::sort_to_local_joint_order( } } -bool JointTrajectoryController::validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const -{ - if (allow_empty && vector_field.empty()) - { - return true; - } - if (joint_names_size != vector_field.size()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size, - string_for_vector_field.c_str(), vector_field.size(), i); - return false; - } - return true; -} - -bool JointTrajectoryController::validate_trajectory_msg( - const trajectory_msgs::msg::JointTrajectory & trajectory) const -{ - // If partial joints goals are not allowed, goal should specify all controller joints - if (!params_.allow_partial_joints_goal) - { - if (trajectory.joint_names.size() != dof_) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Joints on incoming trajectory don't match the controller joints."); - return false; - } - } - - if (trajectory.joint_names.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); - return false; - } - - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) - { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } - if (trajectory_end_time < get_node()->now()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non-zero start time (%f) that ends in the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); - return false; - } - } - - for (size_t i = 0; i < trajectory.joint_names.size(); ++i) - { - const std::string & incoming_joint_name = trajectory.joint_names[i]; - - auto it = std::find(params_.joints.begin(), params_.joints.end(), incoming_joint_name); - if (it == params_.joints.end()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", - incoming_joint_name.c_str()); - return false; - } - } - - if (trajectory.points.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); - return false; - } - - if (!params_.allow_nonzero_velocity_at_trajectory_end) - { - for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) - { - if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits::epsilon()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Velocity of last trajectory point of joint %s is not zero: %.15f", - 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) - { - if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", - i - 1, i, previous_traj_time.seconds(), - rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); - return false; - } - previous_traj_time = trajectory.points[i].time_from_start; - - const size_t joint_count = trajectory.joint_names.size(); - const auto & points = trajectory.points; - // This currently supports only position, velocity and acceleration inputs - if (params_.allow_integration_in_goal_trajectories) - { - const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && - points[i].accelerations.empty(); - const bool position_error = - !points[i].positions.empty() && - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); - const bool velocity_error = - !points[i].velocities.empty() && - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); - const bool acceleration_error = - !points[i].accelerations.empty() && - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, false); - if (all_empty || position_error || velocity_error || acceleration_error) - { - return false; - } - } - else if ( - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true)) - { - return false; - } - // reject effort entries - if (!points[i].effort.empty()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Trajectories with effort fields are currently not supported."); - return false; - } - } - return true; -} - void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b00bd5380..8669d2b347 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1317,169 +1317,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe executor.cancel(); } -/** - * @brief invalid_message Test mismatched joint and reference vector sizes - */ -TEST_P(TrajectoryControllerTestParameterized, invalid_message) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); - rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController( - executor, {partial_joints_parameters, allow_integration_parameters}); - - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - - // Incompatible joint names - traj_msg = good_traj_msg; - traj_msg.joint_names = {"bad_name"}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // empty message - traj_msg = good_traj_msg; - traj_msg.points.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // No position data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too many positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few velocities - traj_msg = good_traj_msg; - traj_msg.points[0].velocities = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few accelerations - traj_msg = good_traj_msg; - traj_msg.points[0].accelerations = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Effort is not supported in trajectory message - traj_msg = good_traj_msg; - traj_msg.points[0].effort = {1.0, 2.0, 3.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Non-strictly increasing waypoint times - traj_msg = good_traj_msg; - traj_msg.points.push_back(traj_msg.points.front()); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} - -/** - * @brief Test invalid velocity at trajectory end with parameter set to false - */ -TEST_P( - TrajectoryControllerTestParameterized, - expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) -{ - rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); - - trajectory_msgs::msg::JointTrajectory traj_msg; - traj_msg.joint_names = joint_names_; - traj_msg.header.stamp = rclcpp::Time(0); - - // empty message (no throw!) - ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Nonzero velocity at trajectory end! - traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(1); - traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - traj_msg.points[0].velocities.resize(1); - traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} - -/** - * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes - * - * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or - * velocities are accepted - */ -TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) -{ - rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters}); - - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - good_traj_msg.points[0].accelerations.resize(1); - good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - - // No position data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // No position and velocity data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].velocities.clear(); - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // All empty - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].velocities.clear(); - traj_msg.points[0].accelerations.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too many positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few velocities - traj_msg = good_traj_msg; - traj_msg.points[0].velocities = {1.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few accelerations - traj_msg = good_traj_msg; - traj_msg.points[0].accelerations = {2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} - /** * @brief test_trajectory_replace Test replacing an existing trajectory */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f049bc65a9..c7106417ca 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -52,7 +52,6 @@ class TestableJointTrajectoryController { public: using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController; - using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg; controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp new file mode 100644 index 0000000000..55d7508f07 --- /dev/null +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -0,0 +1,234 @@ +// Copyright 2024 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "joint_trajectory_controller/trajectory_operations.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/time.hpp" +#include "std_msgs/msg/header.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "test_trajectory_controller_utils.hpp" + +using joint_trajectory_controller::validate_trajectory_msg; +// The fixture for testing class. +class TrajectoryOperationsTest : public testing::Test +{ +protected: + // You can remove any or all of the following functions if their bodies would + // be empty. + + TrajectoryOperationsTest() + { + // You can do set-up work for each test here. + } + + ~TrajectoryOperationsTest() override + { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + + void SetUp() override + { + // Code here will be called immediately after the constructor (right + // before each test). + joint_names_ = {"joint1", "joint2", "joint3"}; + logger_ = std::make_shared(rclcpp::get_logger("test_trajectory_operations")); + + params_.joints = joint_names_; + params_.allow_nonzero_velocity_at_trajectory_end = true; + } + + void TearDown() override + { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Class members declared here can be used by all tests in the test suite + std::vector joint_names_; + rclcpp::Clock clock_; + joint_trajectory_controller::Params params_; + std::shared_ptr logger_; +}; + +/** + * @brief invalid_message Test mismatched joint and reference vector sizes + */ +TEST_F(TrajectoryOperationsTest, invalid_message) +{ + params_.allow_partial_joints_goal = false; + params_.allow_integration_in_goal_trajectories = false; + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_TRUE(validate_trajectory_msg(good_traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible joint names + traj_msg = good_traj_msg; + traj_msg.joint_names = {"bad_name"}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // empty message + traj_msg = good_traj_msg; + traj_msg.points.clear(); + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0, 2.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {1.0, 2.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Effort is not supported in trajectory message + traj_msg = good_traj_msg; + traj_msg.points[0].effort = {1.0, 2.0, 3.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Non-strictly increasing waypoint times + traj_msg = good_traj_msg; + traj_msg.points.push_back(traj_msg.points.front()); + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); +} + +/** + * @brief Test invalid velocity at trajectory end with parameter set to false + */ +TEST_F( + TrajectoryOperationsTest, + expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) +{ + params_.allow_nonzero_velocity_at_trajectory_end = false; + + trajectory_msgs::msg::JointTrajectory traj_msg; + traj_msg.joint_names = joint_names_; + traj_msg.header.stamp = rclcpp::Time(0); + + // empty message (no throw!) + ASSERT_NO_THROW(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Nonzero velocity at trajectory end! + traj_msg.points.resize(1); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); +} + +/** + * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocities are accepted + */ +TEST_F(TrajectoryOperationsTest, missing_positions_message_accepted) +{ + params_.allow_integration_in_goal_trajectories = true; + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + good_traj_msg.points[0].accelerations.resize(1); + good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; + EXPECT_TRUE(validate_trajectory_msg(good_traj_msg, *logger_, clock_.now(), params_)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_TRUE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // No position and velocity data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + EXPECT_TRUE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // All empty + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + traj_msg.points[0].accelerations.clear(); + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {2.0}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); +} From 2f8ae3d8569ff83bff3de16188543a0667731162 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Apr 2024 22:05:44 +0000 Subject: [PATCH 2/9] Improve test coverage --- .../test/test_trajectory_operations.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp index 55d7508f07..dd9030118f 100644 --- a/joint_trajectory_controller/test/test_trajectory_operations.cpp +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -101,6 +101,14 @@ TEST_F(TrajectoryOperationsTest, invalid_message) traj_msg = good_traj_msg; traj_msg.joint_names = {"bad_name"}; EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + traj_msg = good_traj_msg; + traj_msg.joint_names = {"bad_name1", "bad_name2", "bad_name3"}; + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); + + // Empty joint names + traj_msg = good_traj_msg; + traj_msg.joint_names.clear(); + EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); // empty message traj_msg = good_traj_msg; From cac97b693e24db39284f564d6b9fd36d9f523805 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Apr 2024 22:06:43 +0000 Subject: [PATCH 3/9] Move sort_to_local_joint_order to trajectory_operations --- .../joint_trajectory_controller.hpp | 4 - .../trajectory.hpp | 32 ------- .../trajectory_operations.hpp | 94 +++++++++++++++++-- .../src/joint_trajectory_controller.cpp | 51 +--------- 4 files changed, 85 insertions(+), 96 deletions(-) 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 ff6912d51c..7fe32d6a85 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 @@ -231,10 +231,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void fill_partial_goal( std::shared_ptr trajectory_msg) const; - // sorts the joints of the incoming message to our local order - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void sort_to_local_joint_order( - std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void add_new_trajectory_msg( const std::shared_ptr & traj_msg); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index b00d79481c..59ac1d3319 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -162,38 +162,6 @@ class Trajectory bool sampled_already_ = false; }; -/** - * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 - * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated - * mapping vector is "{2, 1}". - */ -template -inline std::vector mapping(const T & t1, const T & t2) -{ - // t1 must be a subset of t2 - if (t1.size() > t2.size()) - { - return std::vector(); - } - - std::vector mapping_vector(t1.size()); // Return value - for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) - { - auto t2_it = std::find(t2.begin(), t2.end(), *t1_it); - if (t2.end() == t2_it) - { - return std::vector(); - } - else - { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); - mapping_vector[t1_dist] = t2_dist; - } - } - return mapping_vector; -} - /** * \param current_position The current position given from the controller, which will be adapted. * \param next_position Next position from which to compute the wraparound offset, i.e., diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp index 9d167bb581..c557467cf6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -34,15 +35,88 @@ namespace joint_trajectory_controller { using namespace std::chrono_literals; // NOLINT -// // fill trajectory_msg so it matches joints controlled by this controller -// // positions set to current position, velocities, accelerations and efforts to 0.0 -// JOINT_TRAJECTORY_CONTROLLER_PUBLIC -// void fill_partial_goal( -// std::shared_ptr trajectory_msg) const; -// // sorts the joints of the incoming message to our local order -// JOINT_TRAJECTORY_CONTROLLER_PUBLIC -// void sort_to_local_joint_order( -// std::shared_ptr trajectory_msg) const; +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated + * mapping vector is "{2, 1}". + */ +template +inline std::vector mapping(const T & t1, const T & t2) +{ + // t1 must be a subset of t2 + if (t1.size() > t2.size()) + { + return std::vector(); + } + + std::vector mapping_vector(t1.size()); // Return value + for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { + auto t2_it = std::find(t2.begin(), t2.end(), *t1_it); + if (t2.end() == t2_it) + { + return std::vector(); + } + else + { + const size_t t1_dist = std::distance(t1.begin(), t1_it); + const size_t t2_dist = std::distance(t2.begin(), t2_it); + mapping_vector[t1_dist] = t2_dist; + } + } + return mapping_vector; +} + +// sorts the joints of the incoming message to our local order +JOINT_TRAJECTORY_CONTROLLER_PUBLIC +void sort_to_local_joint_order( + std::shared_ptr trajectory_msg, rclcpp::Logger logger, + const Params & params) +{ + // rearrange all points in the trajectory message based on mapping + std::vector mapping_vector = mapping(trajectory_msg->joint_names, params.joints); + auto remap = [logger, params]( + const std::vector & to_remap, + const std::vector & mapping) -> std::vector + { + if (to_remap.empty()) + { + return to_remap; + } + if (to_remap.size() != mapping.size()) + { + RCLCPP_WARN(logger, "Invalid input size (%zu) for sorting", to_remap.size()); + return to_remap; + } + static std::vector output(params.joints.size(), 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]; + output[map_index] = to_remap[index]; + } + return output; + }; + + for (size_t index = 0; index < trajectory_msg->points.size(); ++index) + { + trajectory_msg->points[index].positions = + remap(trajectory_msg->points[index].positions, mapping_vector); + + trajectory_msg->points[index].velocities = + remap(trajectory_msg->points[index].velocities, mapping_vector); + + trajectory_msg->points[index].accelerations = + remap(trajectory_msg->points[index].accelerations, mapping_vector); + + trajectory_msg->points[index].effort = + remap(trajectory_msg->points[index].effort, mapping_vector); + } +} JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( @@ -67,7 +141,7 @@ bool validate_trajectory_point_field( JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory, rclcpp::Logger logger, rclcpp::Time now, - const Params params) + const Params & params) { // If partial joints goals are not allowed, goal should specify all controller joints if (!params.allow_partial_joints_goal) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ad6ff25bf9..8edd67d75c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -151,7 +151,7 @@ controller_interface::return_type JointTrajectoryController::update( (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); + sort_to_local_joint_order(*new_external_msg, get_node()->get_logger(), params_); // TODO(denis): Add here integration of position and velocity traj_external_point_ptr_->update(*new_external_msg); } @@ -1282,55 +1282,6 @@ void JointTrajectoryController::fill_partial_goal( } } -void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) const -{ - // rearrange all points in the trajectory message based on mapping - std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); - auto remap = [this]( - const std::vector & to_remap, - const std::vector & mapping) -> std::vector - { - if (to_remap.empty()) - { - return to_remap; - } - if (to_remap.size() != mapping.size()) - { - RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); - return to_remap; - } - static std::vector 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]; - output[map_index] = to_remap[index]; - } - return output; - }; - - for (size_t index = 0; index < trajectory_msg->points.size(); ++index) - { - trajectory_msg->points[index].positions = - remap(trajectory_msg->points[index].positions, mapping_vector); - - trajectory_msg->points[index].velocities = - remap(trajectory_msg->points[index].velocities, mapping_vector); - - trajectory_msg->points[index].accelerations = - remap(trajectory_msg->points[index].accelerations, mapping_vector); - - trajectory_msg->points[index].effort = - remap(trajectory_msg->points[index].effort, mapping_vector); - } -} - void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { From 807e16d3be781e06eb7dd48db714aba357e8bff1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 25 Apr 2024 08:18:01 +0000 Subject: [PATCH 4/9] Move fill_partial_goal --- .../joint_trajectory_controller.hpp | 5 -- .../trajectory_operations.hpp | 54 ++++++++++++ .../src/joint_trajectory_controller.cpp | 84 +++++-------------- 3 files changed, 77 insertions(+), 66 deletions(-) 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 7fe32d6a85..4d60790138 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 @@ -226,11 +226,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void compute_error_for_joint( JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) const; - // fill trajectory_msg so it matches joints controlled by this controller - // positions set to current position, velocities, accelerations and efforts to 0.0 - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void fill_partial_goal( - std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void add_new_trajectory_msg( const std::shared_ptr & traj_msg); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp index c557467cf6..eeba984783 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp @@ -118,6 +118,60 @@ void sort_to_local_joint_order( } } +// fill trajectory_msg so it matches joints controlled by this controller +// positions set to current position, velocities, accelerations and efforts to 0.0 +JOINT_TRAJECTORY_CONTROLLER_PUBLIC +void fill_partial_goal( + std::shared_ptr trajectory_msg, + std::function get_default_position, const Params & params) +{ + const auto dof = params.joints.size(); + // joint names in the goal are a subset of existing joints, as checked in goal_callback + // so if the size matches, the goal contains all controller joints + if (dof == trajectory_msg->joint_names.size()) + { + return; + } + + trajectory_msg->joint_names.reserve(dof); + + for (size_t index = 0; index < dof; ++index) + { + { + if ( + std::find( + trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), + params.joints[index]) != trajectory_msg->joint_names.end()) + { + // joint found on msg + continue; + } + trajectory_msg->joint_names.push_back(params.joints[index]); + + for (auto & it : trajectory_msg->points) + { + // Assume hold position with 0 velocity and acceleration for missing joints + if (!it.positions.empty()) + { + it.positions.push_back(get_default_position(index)); + } + if (!it.velocities.empty()) + { + it.velocities.push_back(0.0); + } + if (!it.accelerations.empty()) + { + it.accelerations.push_back(0.0); + } + if (!it.effort.empty()) + { + it.effort.push_back(0.0); + } + } + } + } +} + JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( size_t joint_names_size, const std::vector & vector_field, diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 8edd67d75c..b96b6d2414 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -150,7 +150,29 @@ controller_interface::return_type JointTrajectoryController::update( current_external_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { - fill_partial_goal(*new_external_msg); + fill_partial_goal( + *new_external_msg, + [this](size_t index) + { + if ( + has_position_command_interface_ && + !std::isnan(joint_command_interface_[0][index].get().get_value())) + { + // copy last command if cmd interface exists + return joint_command_interface_[0][index].get().get_value(); + } + else if (has_position_state_interface_) + { + // copy current state if state interface exists + return joint_state_interface_[0][index].get().get_value(); + } + else + { + // this should never happen + return std::numeric_limits::quiet_NaN(); + } + }, + params_); sort_to_local_joint_order(*new_external_msg, get_node()->get_logger(), params_); // TODO(denis): Add here integration of position and velocity traj_external_point_ptr_->update(*new_external_msg); @@ -1222,66 +1244,6 @@ void JointTrajectoryController::compute_error_for_joint( } } -void JointTrajectoryController::fill_partial_goal( - std::shared_ptr trajectory_msg) const -{ - // joint names in the goal are a subset of existing joints, as checked in goal_callback - // so if the size matches, the goal contains all controller joints - if (dof_ == trajectory_msg->joint_names.size()) - { - return; - } - - trajectory_msg->joint_names.reserve(dof_); - - for (size_t index = 0; index < dof_; ++index) - { - { - if ( - std::find( - trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), - params_.joints[index]) != trajectory_msg->joint_names.end()) - { - // joint found on msg - continue; - } - trajectory_msg->joint_names.push_back(params_.joints[index]); - - for (auto & it : trajectory_msg->points) - { - // Assume hold position with 0 velocity and acceleration for missing joints - if (!it.positions.empty()) - { - if ( - has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) - { - // copy last command if cmd interface exists - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); - } - else if (has_position_state_interface_) - { - // copy current state if state interface exists - it.positions.push_back(joint_state_interface_[0][index].get().get_value()); - } - } - if (!it.velocities.empty()) - { - it.velocities.push_back(0.0); - } - if (!it.accelerations.empty()) - { - it.accelerations.push_back(0.0); - } - if (!it.effort.empty()) - { - it.effort.push_back(0.0); - } - } - } - } -} - void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { From 1e0575aed43f0d9eaef205eb3f1504bb6f91610b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 25 Apr 2024 12:22:45 +0000 Subject: [PATCH 5/9] Also sort the joint_names --- .../joint_trajectory_controller/trajectory_operations.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp index eeba984783..2411670caa 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp @@ -102,6 +102,9 @@ void sort_to_local_joint_order( return output; }; + // might not be used any more, but to get a genuine msg + trajectory_msg->joint_names = params.joints; + for (size_t index = 0; index < trajectory_msg->points.size(); ++index) { trajectory_msg->points[index].positions = From 7b44b6bb43e0f8f40b55bd22ea4f9909801fcf82 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 25 Apr 2024 12:23:09 +0000 Subject: [PATCH 6/9] Add test for sort_to_local_joint_order --- .../test/test_trajectory_operations.cpp | 144 +++++++++++++++++- 1 file changed, 137 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp index dd9030118f..7f1bbe6269 100644 --- a/joint_trajectory_controller/test/test_trajectory_operations.cpp +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -32,7 +32,87 @@ #include "test_trajectory_controller_utils.hpp" +using joint_trajectory_controller::sort_to_local_joint_order; using joint_trajectory_controller::validate_trajectory_msg; + +std::string vectorToString(const std::vector & vec) +{ + std::ostringstream oss; + for (const auto & val : vec) + { + oss << val << " "; + } + return oss.str(); +} + +// Custom matcher to check if a trajectory matches the expected joint names and values +MATCHER_P2(IsTrajMsgSorted, joint_names, vector_val, "") +{ + if (arg->joint_names != joint_names) + { + *result_listener << "Joint names mismatch"; + return false; + } + + if (arg->points.size() == 0) + { + *result_listener << "Trajectory has no points"; + return false; + } + + const auto & point = arg->points[0]; + if ( + point.positions != vector_val || point.velocities != vector_val || + point.accelerations != vector_val || point.effort != vector_val) + { + *result_listener << "Point values mismatch:"; + if (point.positions != vector_val) + { + *result_listener << "\npositions: " << vectorToString(point.positions) << " ne " + << vectorToString(vector_val); + } + if (point.velocities != vector_val) + { + *result_listener << "\nvelocities: " << vectorToString(point.velocities) << " ne " + << vectorToString(vector_val); + } + if (point.accelerations != vector_val) + { + *result_listener << "\naccelerations: " << vectorToString(point.accelerations) << " ne " + << vectorToString(vector_val); + } + if (point.effort != vector_val) + { + *result_listener << "\neffort: " << vectorToString(point.effort) << " ne " + << vectorToString(vector_val); + } + return false; + } + + return true; +} + +template +auto get_jumbled_values = [](const std::vector & jumble_map, const std::vector & values) +{ + std::vector result; + for (const auto & index : jumble_map) + { + result.push_back(values[index]); + } + return result; +}; +auto get_jumbled_string = + [](const std::vector & jumble_map, const testing::internal::Strings & values) +{ + testing::internal::Strings result; + for (const auto & index : jumble_map) + { + result.push_back(values[index]); + } + return result; +}; + // The fixture for testing class. class TrajectoryOperationsTest : public testing::Test { @@ -91,9 +171,7 @@ TEST_F(TrajectoryOperationsTest, invalid_message) good_traj_msg.header.stamp = rclcpp::Time(0); good_traj_msg.points.resize(1); good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; EXPECT_TRUE(validate_trajectory_msg(good_traj_msg, *logger_, clock_.now(), params_)); @@ -171,9 +249,7 @@ TEST_F( // Nonzero velocity at trajectory end! traj_msg.points.resize(1); traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(1); traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - traj_msg.points[0].velocities.resize(1); traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); } @@ -194,11 +270,8 @@ TEST_F(TrajectoryOperationsTest, missing_positions_message_accepted) good_traj_msg.header.stamp = rclcpp::Time(0); good_traj_msg.points.resize(1); good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - good_traj_msg.points[0].accelerations.resize(1); good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; EXPECT_TRUE(validate_trajectory_msg(good_traj_msg, *logger_, clock_.now(), params_)); @@ -240,3 +313,60 @@ TEST_F(TrajectoryOperationsTest, missing_positions_message_accepted) traj_msg.points[0].accelerations = {2.0}; EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_)); } + +/** + * @brief test_msg_sorting test sort_to_local_joint_order() + * + */ +TEST_F(TrajectoryOperationsTest, test_msg_sorting) +{ + std::vector jumble_map = {1, 2, 0}; + auto vector_val = std::vector{1.0, 2.0, 3.0}; + + trajectory_msgs::msg::JointTrajectory good_traj_msg, traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions = vector_val; + good_traj_msg.points[0].velocities = vector_val; + good_traj_msg.points[0].accelerations = vector_val; + good_traj_msg.points[0].effort = vector_val; + + // test if the joint order is not changed + { + auto traj_msg_ptr = std::make_shared(good_traj_msg); + EXPECT_THAT(traj_msg_ptr, IsTrajMsgSorted(joint_names_, vector_val)); + ASSERT_NO_THROW(sort_to_local_joint_order(traj_msg_ptr, *logger_, params_)); + EXPECT_THAT(traj_msg_ptr, IsTrajMsgSorted(joint_names_, vector_val)); + } + + // test if the joint order is sorted + traj_msg = good_traj_msg; + traj_msg.joint_names = get_jumbled_string(jumble_map, joint_names_); + traj_msg.points[0].positions = get_jumbled_values(jumble_map, vector_val); + traj_msg.points[0].velocities = get_jumbled_values(jumble_map, vector_val); + traj_msg.points[0].accelerations = get_jumbled_values(jumble_map, vector_val); + traj_msg.points[0].effort = get_jumbled_values(jumble_map, vector_val); + { + auto traj_msg_ptr = std::make_shared(traj_msg); + EXPECT_THAT(traj_msg_ptr, testing::Not(IsTrajMsgSorted(joint_names_, vector_val))); + ASSERT_NO_THROW(sort_to_local_joint_order(traj_msg_ptr, *logger_, params_)); + EXPECT_THAT(traj_msg_ptr, IsTrajMsgSorted(joint_names_, vector_val)); + } + + // test if no issue if called with different size (should not happen, because no valid_traj_msg) + traj_msg = good_traj_msg; + traj_msg.joint_names = + std::vector{joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; + ASSERT_NO_THROW(sort_to_local_joint_order( + std::make_shared(traj_msg), *logger_, params_)); + traj_msg = good_traj_msg; + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].accelerations.resize(1); + traj_msg.points[0].effort.resize(1); + ASSERT_NO_THROW(sort_to_local_joint_order( + std::make_shared(traj_msg), *logger_, params_)); +} From 64f3e703e6ec560bc6b087221733fe4fa429ed61 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 25 Apr 2024 12:26:53 +0000 Subject: [PATCH 7/9] Simplify code --- .../test/test_trajectory_operations.cpp | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp index 7f1bbe6269..afdc744ccd 100644 --- a/joint_trajectory_controller/test/test_trajectory_operations.cpp +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -102,16 +102,6 @@ auto get_jumbled_values = [](const std::vector & jumble_map, const std:: } return result; }; -auto get_jumbled_string = - [](const std::vector & jumble_map, const testing::internal::Strings & values) -{ - testing::internal::Strings result; - for (const auto & index : jumble_map) - { - result.push_back(values[index]); - } - return result; -}; // The fixture for testing class. class TrajectoryOperationsTest : public testing::Test @@ -344,7 +334,7 @@ TEST_F(TrajectoryOperationsTest, test_msg_sorting) // test if the joint order is sorted traj_msg = good_traj_msg; - traj_msg.joint_names = get_jumbled_string(jumble_map, joint_names_); + traj_msg.joint_names = get_jumbled_values(jumble_map, joint_names_); traj_msg.points[0].positions = get_jumbled_values(jumble_map, vector_val); traj_msg.points[0].velocities = get_jumbled_values(jumble_map, vector_val); traj_msg.points[0].accelerations = get_jumbled_values(jumble_map, vector_val); @@ -358,8 +348,7 @@ TEST_F(TrajectoryOperationsTest, test_msg_sorting) // test if no issue if called with different size (should not happen, because no valid_traj_msg) traj_msg = good_traj_msg; - traj_msg.joint_names = - std::vector{joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; + traj_msg.joint_names = get_jumbled_values({1, 2}, joint_names_); ASSERT_NO_THROW(sort_to_local_joint_order( std::make_shared(traj_msg), *logger_, params_)); traj_msg = good_traj_msg; From 4ede68c22ecabe8392c3877cb962cc184cf2aac9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 25 Apr 2024 13:09:23 +0000 Subject: [PATCH 8/9] Add test for fill_partial_goal --- .../test/test_trajectory_operations.cpp | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp index afdc744ccd..617aa3715d 100644 --- a/joint_trajectory_controller/test/test_trajectory_operations.cpp +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -32,6 +32,7 @@ #include "test_trajectory_controller_utils.hpp" +using joint_trajectory_controller::fill_partial_goal; using joint_trajectory_controller::sort_to_local_joint_order; using joint_trajectory_controller::validate_trajectory_msg; @@ -359,3 +360,57 @@ TEST_F(TrajectoryOperationsTest, test_msg_sorting) ASSERT_NO_THROW(sort_to_local_joint_order( std::make_shared(traj_msg), *logger_, params_)); } + +TEST_F(TrajectoryOperationsTest, TestFillPartialGoal) +{ + // Create a trajectory message + auto vector_val = std::vector{1.0, 2.0, 3.0}; + trajectory_msgs::msg::JointTrajectory traj_msg_partial; + std::vector jumble_map = {0, 2}; + traj_msg_partial.joint_names = get_jumbled_values(jumble_map, joint_names_); + traj_msg_partial.header.stamp = rclcpp::Time(0); + traj_msg_partial.points.resize(1); + traj_msg_partial.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg_partial.points[0].positions = get_jumbled_values(jumble_map, vector_val); + traj_msg_partial.points[0].velocities = get_jumbled_values(jumble_map, vector_val); + traj_msg_partial.points[0].accelerations = get_jumbled_values(jumble_map, vector_val); + traj_msg_partial.points[0].effort = get_jumbled_values(jumble_map, vector_val); + auto traj_msg_ptr = std::make_shared(traj_msg_partial); + + // Function to return default position for a joint + auto get_default_position = [](size_t index) { return 10. + static_cast(index); }; + + // Call fill_partial_goal function + fill_partial_goal(traj_msg_ptr, get_default_position, params_); + + // Check if the fields are correctly filled (missing joints are added at the end) + EXPECT_THAT( + traj_msg_ptr->joint_names, + testing::ContainerEq(get_jumbled_values({0, 2, 1}, joint_names_))); + ASSERT_THAT(traj_msg_ptr->points, testing::SizeIs(1)); + const auto point = traj_msg_ptr->points[0]; + ASSERT_THAT(point.positions, testing::SizeIs(params_.joints.size())); + ASSERT_THAT(point.velocities, testing::SizeIs(params_.joints.size())); + ASSERT_THAT(point.accelerations, testing::SizeIs(params_.joints.size())); + ASSERT_THAT(point.effort, testing::SizeIs(params_.joints.size())); + for (size_t i = 0; i < jumble_map.size(); ++i) + { + // should be original points + EXPECT_EQ(point.positions[i], traj_msg_partial.points[0].positions[i]); + EXPECT_EQ(point.velocities[i], traj_msg_partial.points[0].velocities[i]); + EXPECT_EQ(point.accelerations[i], traj_msg_partial.points[0].accelerations[i]); + EXPECT_EQ(point.effort[i], traj_msg_partial.points[0].effort[i]); + } + for (size_t i = jumble_map.size(); i < params_.joints.size(); ++i) + { + // Check if positions are set to default position, and velocities, accelerations, and efforts + // are set to 0.0 + auto it = std::find(params_.joints.begin(), params_.joints.end(), traj_msg_ptr->joint_names[i]); + ASSERT_NE(it, params_.joints.end()) << "Joint " << traj_msg_ptr->joint_names[i] << " not found"; + auto index = std::distance(params_.joints.begin(), it); + EXPECT_EQ(point.positions[i], get_default_position(index)); + EXPECT_EQ(point.velocities[i], 0.0); + EXPECT_EQ(point.accelerations[i], 0.0); + EXPECT_EQ(point.effort[i], 0.0); + } +} From 18e264387898f49ce88d1d15413dfc8afd746f7e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 29 Apr 2024 12:08:39 +0000 Subject: [PATCH 9/9] Use common function name style --- joint_trajectory_controller/test/test_trajectory_operations.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp index 617aa3715d..ff0fa3eb99 100644 --- a/joint_trajectory_controller/test/test_trajectory_operations.cpp +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -361,7 +361,7 @@ TEST_F(TrajectoryOperationsTest, test_msg_sorting) std::make_shared(traj_msg), *logger_, params_)); } -TEST_F(TrajectoryOperationsTest, TestFillPartialGoal) +TEST_F(TrajectoryOperationsTest, test_fill_partial_goal) { // Create a trajectory message auto vector_val = std::vector{1.0, 2.0, 3.0};