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..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 @@ -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" @@ -225,24 +226,9 @@ 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_msgs::msg::JointTrajectory> 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_msgs::msg::JointTrajectory> 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<trajectory_msgs::msg::JointTrajectory> & traj_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_point_field( - size_t joint_names_size, const std::vector<double> & 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.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 <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated - * mapping vector is <tt>"{2, 1}"</tt>. - */ -template <class T> -inline std::vector<size_t> mapping(const T & t1, const T & t2) -{ - // t1 must be a subset of t2 - if (t1.size() > t2.size()) - { - return std::vector<size_t>(); - } - - std::vector<size_t> 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<size_t>(); - } - 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 new file mode 100644 index 0000000000..2411670caa --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp @@ -0,0 +1,335 @@ +// 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 <chrono> +#include <cmath> +#include <limits> +#include <memory> +#include <string> +#include <vector> + +#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 + +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated + * mapping vector is <tt>"{2, 1}"</tt>. + */ +template <class T> +inline std::vector<size_t> mapping(const T & t1, const T & t2) +{ + // t1 must be a subset of t2 + if (t1.size() > t2.size()) + { + return std::vector<size_t>(); + } + + std::vector<size_t> 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<size_t>(); + } + 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_msgs::msg::JointTrajectory> trajectory_msg, rclcpp::Logger logger, + const Params & params) +{ + // rearrange all points in the trajectory message based on mapping + std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params.joints); + auto remap = [logger, params]( + const std::vector<double> & to_remap, + const std::vector<size_t> & mapping) -> std::vector<double> + { + 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<double> 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; + }; + + // 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 = + 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); + } +} + +// 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_msgs::msg::JointTrajectory> trajectory_msg, + std::function<double(size_t)> 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<double> & 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<rclcpp::Time>(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<float>::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..b96b6d2414 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -150,8 +150,30 @@ 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); - sort_to_local_joint_order(*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<double>::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); } @@ -1103,7 +1125,7 @@ void JointTrajectoryController::publish_state( void JointTrajectoryController::topic_callback( const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg) { - if (!validate_trajectory_msg(*msg)) + if (!validate_trajectory_msg(*msg, get_node()->get_logger(), get_node()->now(), params_)) { return; } @@ -1129,7 +1151,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; } @@ -1221,266 +1244,6 @@ void JointTrajectoryController::compute_error_for_joint( } } -void JointTrajectoryController::fill_partial_goal( - std::shared_ptr<trajectory_msgs::msg::JointTrajectory> 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::sort_to_local_joint_order( - std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const -{ - // rearrange all points in the trajectory message based on mapping - std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); - auto remap = [this]( - const std::vector<double> & to_remap, - const std::vector<size_t> & mapping) -> std::vector<double> - { - 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<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]; - 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); - } -} - -bool JointTrajectoryController::validate_trajectory_point_field( - size_t joint_names_size, const std::vector<double> & 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<rclcpp::Time>(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<float>::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<trajectory_msgs::msg::JointTrajectory> & 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..ff0fa3eb99 --- /dev/null +++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp @@ -0,0 +1,416 @@ +// 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 <chrono> +#include <cmath> +#include <memory> +#include <vector> + +#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::fill_partial_goal; +using joint_trajectory_controller::sort_to_local_joint_order; +using joint_trajectory_controller::validate_trajectory_msg; + +std::string vectorToString(const std::vector<double> & 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 <typename T> +auto get_jumbled_values = [](const std::vector<size_t> & jumble_map, const std::vector<T> & values) +{ + std::vector<T> result; + for (const auto & index : jumble_map) + { + result.push_back(values[index]); + } + return result; +}; + +// 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::Logger>(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<std::string> joint_names_; + rclcpp::Clock clock_; + joint_trajectory_controller::Params params_; + std::shared_ptr<rclcpp::Logger> 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 = {1.0, 2.0, 3.0}; + 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_)); + 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; + 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 = {1.0, 2.0, 3.0}; + 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 = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + 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_)); +} + +/** + * @brief test_msg_sorting test sort_to_local_joint_order() + * + */ +TEST_F(TrajectoryOperationsTest, test_msg_sorting) +{ + std::vector<size_t> jumble_map = {1, 2, 0}; + auto vector_val = std::vector<double>{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<trajectory_msgs::msg::JointTrajectory>(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_values<std::string>(jumble_map, joint_names_); + traj_msg.points[0].positions = get_jumbled_values<double>(jumble_map, vector_val); + traj_msg.points[0].velocities = get_jumbled_values<double>(jumble_map, vector_val); + traj_msg.points[0].accelerations = get_jumbled_values<double>(jumble_map, vector_val); + traj_msg.points[0].effort = get_jumbled_values<double>(jumble_map, vector_val); + { + auto traj_msg_ptr = std::make_shared<trajectory_msgs::msg::JointTrajectory>(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 = get_jumbled_values<std::string>({1, 2}, joint_names_); + ASSERT_NO_THROW(sort_to_local_joint_order( + std::make_shared<trajectory_msgs::msg::JointTrajectory>(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<trajectory_msgs::msg::JointTrajectory>(traj_msg), *logger_, params_)); +} + +TEST_F(TrajectoryOperationsTest, test_fill_partial_goal) +{ + // Create a trajectory message + auto vector_val = std::vector<double>{1.0, 2.0, 3.0}; + trajectory_msgs::msg::JointTrajectory traj_msg_partial; + std::vector<size_t> jumble_map = {0, 2}; + traj_msg_partial.joint_names = get_jumbled_values<std::string>(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<double>(jumble_map, vector_val); + traj_msg_partial.points[0].velocities = get_jumbled_values<double>(jumble_map, vector_val); + traj_msg_partial.points[0].accelerations = get_jumbled_values<double>(jumble_map, vector_val); + traj_msg_partial.points[0].effort = get_jumbled_values<double>(jumble_map, vector_val); + auto traj_msg_ptr = std::make_shared<trajectory_msgs::msg::JointTrajectory>(traj_msg_partial); + + // Function to return default position for a joint + auto get_default_position = [](size_t index) { return 10. + static_cast<double>(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<std::string>({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); + } +}