diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp new file mode 100644 index 0000000000..bc4148905a --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/cartesian_trajectory_generator_state.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace cartesian_trajectory_generator +{ +class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTrajectoryController +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + CartesianTrajectoryGenerator(); + + /** + * @brief command_interface_configuration This controller requires the position and velocity + * state interfaces for the controlled joints + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + using ControllerReferenceMsg = trajectory_msgs::msg::MultiDOFJointTrajectoryPoint; + using ControllerFeedbackMsg = nav_msgs::msg::Odometry; + +protected: + bool read_state_from_hardware(JointTrajectoryPoint & state) override; + + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, + const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, + const JointTrajectoryPoint & ruckig_input) override; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_reliable_ = nullptr; + realtime_tools::RealtimeBuffer> reference_world_; + + rclcpp::Subscription::SharedPtr feedback_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> feedback_; + + trajectory_msgs::msg::JointTrajectoryPoint control_output_local_; + +private: + void reference_callback(const std::shared_ptr msg); + + using CartControllerStateMsg = control_msgs::msg::CartesianTrajectoryGeneratorState; + using CartStatePublisher = realtime_tools::RealtimePublisher; + using CartStatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr cart_publisher_; + CartStatePublisherPtr cart_state_publisher_; + + std::vector configured_joint_limits_; + + // storage of last received measured position to + geometry_msgs::msg::Pose last_received_measured_position_; +}; + +} // namespace cartesian_trajectory_generator + +#endif // JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp new file mode 100644 index 0000000000..a84e10f627 --- /dev/null +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -0,0 +1,430 @@ +// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "joint_trajectory_controller/cartesian_trajectory_generator.hpp" + +#include "controller_interface/helpers.hpp" +#include "eigen3/Eigen/Eigen" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "joint_trajectory_controller/trajectory.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +void reset_twist_msg(geometry_msgs::msg::Twist & msg) +{ + msg.linear.x = std::numeric_limits::quiet_NaN(); + msg.linear.y = std::numeric_limits::quiet_NaN(); + msg.linear.z = std::numeric_limits::quiet_NaN(); + msg.angular.x = std::numeric_limits::quiet_NaN(); + msg.angular.y = std::numeric_limits::quiet_NaN(); + msg.angular.z = std::numeric_limits::quiet_NaN(); +} + +using ControllerReferenceMsg = + cartesian_trajectory_generator::CartesianTrajectoryGenerator::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg(ControllerReferenceMsg & msg) +{ + msg.transforms.resize(1); + msg.transforms[0].translation.x = std::numeric_limits::quiet_NaN(); + msg.transforms[0].translation.y = std::numeric_limits::quiet_NaN(); + msg.transforms[0].translation.z = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.x = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.y = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.z = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.w = std::numeric_limits::quiet_NaN(); + + msg.velocities.resize(1); + reset_twist_msg(msg.velocities[0]); + + msg.accelerations.resize(1); + reset_twist_msg(msg.accelerations[0]); +} + +void reset_controller_reference_msg(const std::shared_ptr & msg) +{ + reset_controller_reference_msg(*msg); +} + +void rpy_to_quaternion( + std::array & orientation_angles, geometry_msgs::msg::Quaternion & quaternion_msg) +{ + // convert quaternion to euler angles + tf2::Quaternion quaternion; + quaternion.setRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + quaternion_msg = tf2::toMsg(quaternion); +} +} // namespace + +namespace cartesian_trajectory_generator +{ +CartesianTrajectoryGenerator::CartesianTrajectoryGenerator() +: joint_trajectory_controller::JointTrajectoryController() +{ +} + +controller_interface::InterfaceConfiguration +CartesianTrajectoryGenerator::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::NONE; + return conf; +} + +controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // store joint limits for later + configured_joint_limits_ = joint_limits_; + + // topics QoS + auto qos_best_effort_history_depth_one = rclcpp::SystemDefaultsQoS(); + qos_best_effort_history_depth_one.keep_last(1); + qos_best_effort_history_depth_one.best_effort(); + auto subscribers_reliable_qos = rclcpp::SystemDefaultsQoS(); + subscribers_reliable_qos.keep_all(); + subscribers_reliable_qos.reliable(); + + // Reference Subscribers (reliable channel also for updates not to be missed) + ref_subscriber_ = get_node()->create_subscription( + "~/reference", qos_best_effort_history_depth_one, + std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); + ref_subscriber_reliable_ = get_node()->create_subscription( + "~/reference_reliable", subscribers_reliable_qos, + std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg); + reference_world_.writeFromNonRT(msg); + + // Odometry feedback + auto feedback_callback = [&](const std::shared_ptr feedback_msg) -> void + { feedback_.writeFromNonRT(feedback_msg); }; + feedback_subscriber_ = get_node()->create_subscription( + "~/feedback", qos_best_effort_history_depth_one, feedback_callback); + // initialize feedback to null pointer since it is used to determine if we have valid data or not + feedback_.writeFromNonRT(nullptr); + + // service QoS + auto services_qos = rclcpp::SystemDefaultsQoS(); // message queue depth + services_qos.keep_all(); + services_qos.reliable(); + services_qos.durability_volatile(); + + cart_publisher_ = get_node()->create_publisher( + "~/controller_state_cartesian", qos_best_effort_history_depth_one); + cart_state_publisher_ = std::make_unique(cart_publisher_); + + cart_state_publisher_->lock(); + cart_state_publisher_->msg_.dof_names = params_.joints; + cart_state_publisher_->msg_.reference_world.transforms.resize(1); + cart_state_publisher_->msg_.reference_world.velocities.resize(1); + cart_state_publisher_->msg_.reference_world.accelerations.resize(1); + cart_state_publisher_->msg_.reference_local.transforms.resize(1); + cart_state_publisher_->msg_.reference_local.velocities.resize(1); + cart_state_publisher_->msg_.reference_local.accelerations.resize(1); + cart_state_publisher_->msg_.feedback.transforms.resize(1); + cart_state_publisher_->msg_.feedback.velocities.resize(1); + cart_state_publisher_->msg_.feedback.accelerations.resize(1); + cart_state_publisher_->msg_.feedback_local.transforms.resize(1); + cart_state_publisher_->msg_.feedback_local.velocities.resize(1); + cart_state_publisher_->msg_.feedback_local.accelerations.resize(1); + cart_state_publisher_->msg_.error.transforms.resize(1); + cart_state_publisher_->msg_.error.velocities.resize(1); + cart_state_publisher_->msg_.error.accelerations.resize(1); + cart_state_publisher_->msg_.output_world.transforms.resize(1); + cart_state_publisher_->msg_.output_world.velocities.resize(1); + cart_state_publisher_->msg_.output_world.accelerations.resize(1); + cart_state_publisher_->msg_.output_local.transforms.resize(1); + cart_state_publisher_->msg_.output_local.velocities.resize(1); + cart_state_publisher_->msg_.output_local.accelerations.resize(1); + cart_state_publisher_->unlock(); + + return CallbackReturn::SUCCESS; +} + +void CartesianTrajectoryGenerator::reference_callback( + const std::shared_ptr msg) +{ + // store input ref for later use + reference_world_.writeFromNonRT(msg); + + // assume for now that we are working with trajectories with one point - we don't know exactly + // where we are in the trajectory before sampling - nevertheless this should work for the use case + auto new_traj_msg = std::make_shared(); + new_traj_msg->joint_names = params_.joints; + new_traj_msg->points.resize(1); + new_traj_msg->points[0].positions.resize( + params_.joints.size(), std::numeric_limits::quiet_NaN()); + new_traj_msg->points[0].velocities.resize( + params_.joints.size(), std::numeric_limits::quiet_NaN()); + if (msg->time_from_start.nanosec == 0) + { + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_seconds(0.01); + } + else + { + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_nanoseconds( + static_cast(msg->time_from_start.nanosec)); + } + + // just pass input into trajectory message + auto assign_value_from_input = [&]( + const double pos_from_msg, const double vel_from_msg, + const std::string & joint_name, const size_t index) + { + new_traj_msg->points[0].positions[index] = pos_from_msg; + new_traj_msg->points[0].velocities[index] = vel_from_msg; + if (std::isnan(pos_from_msg) && std::isnan(vel_from_msg)) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Input position and velocity for %s is NaN", joint_name.c_str()); + } + }; + + assign_value_from_input( + msg->transforms[0].translation.x, msg->velocities[0].linear.x, params_.joints[0], 0); + assign_value_from_input( + msg->transforms[0].translation.y, msg->velocities[0].linear.y, params_.joints[1], 1); + assign_value_from_input( + msg->transforms[0].translation.z, msg->velocities[0].linear.z, params_.joints[2], 2); + assign_value_from_input( + msg->transforms[0].rotation.x, msg->velocities[0].angular.x, params_.joints[3], 3); + assign_value_from_input( + msg->transforms[0].rotation.y, msg->velocities[0].angular.y, params_.joints[4], 4); + assign_value_from_input( + msg->transforms[0].rotation.z, msg->velocities[0].angular.z, params_.joints[5], 5); + + add_new_trajectory_msg(new_traj_msg); +} + +controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( + const rclcpp_lifecycle::State &) +{ + // order all joints in the storage + for (const auto & interface : params_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; + } + } + // for (const auto & interface : state_interface_types_) + // { + // auto it = + // std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + // auto index = std::distance(allowed_interface_types_.begin(), it); + // if (!controller_interface::get_ordered_interfaces( + // state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + // { + // RCLCPP_ERROR( + // get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + // interface.c_str(), joint_state_interface_[index].size()); + // return controller_interface::CallbackReturn::ERROR; + // } + // } + + traj_external_point_ptr_ = std::make_shared(); + traj_msg_external_point_ptr_.writeFromNonRT( + std::shared_ptr()); + + subscriber_is_active_ = true; + + // Initialize current state storage if hardware state has tracking offset + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + if (!read_state_from_hardware(state)) return CallbackReturn::ERROR; + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + // Handle restart of controller by reading from commands if + // those are not nan + if (read_state_from_command_interfaces(state)) + { + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + } + + return CallbackReturn::SUCCESS; +} + +bool CartesianTrajectoryGenerator::read_state_from_hardware(JointTrajectoryPoint & state) +{ + std::array orientation_angles; + const auto measured_state = *(feedback_.readFromRT()); + if (!measured_state) return false; + + tf2::Quaternion measured_q; + + if ( + std::isnan(measured_state->pose.pose.orientation.w) || + std::isnan(measured_state->pose.pose.orientation.x) || + std::isnan(measured_state->pose.pose.orientation.y) || + std::isnan(measured_state->pose.pose.orientation.z)) + { + // if any of the orientation is NaN, revert to previous orientation + measured_q.setRPY(state.positions[3], state.positions[4], state.positions[5]); + } + else + { + tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); + } + tf2::Matrix3x3 m(measured_q); + m.getRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + + // Assign values from the hardware + // Position states always exist + // if any measured position is NaN, keep previous value + state.positions[0] = std::isnan(measured_state->pose.pose.position.x) + ? state.positions[0] + : measured_state->pose.pose.position.x; + state.positions[1] = std::isnan(measured_state->pose.pose.position.y) + ? state.positions[1] + : measured_state->pose.pose.position.y; + state.positions[2] = std::isnan(measured_state->pose.pose.position.z) + ? state.positions[2] + : measured_state->pose.pose.position.z; + state.positions[3] = orientation_angles[0]; + state.positions[4] = orientation_angles[1]; + state.positions[5] = orientation_angles[2]; + + // Convert measured twist which is in body frame to world frame since CTG/JTC expects state + // in world frame + + Eigen::Quaterniond q_body_in_world( + measured_q.w(), measured_q.x(), measured_q.y(), measured_q.z()); + + // if any measured linear velocity is NaN, set to zero + Eigen::Vector3d linear_vel_body( + std::isnan(measured_state->twist.twist.linear.x) ? 0.0 : measured_state->twist.twist.linear.x, + std::isnan(measured_state->twist.twist.linear.y) ? 0.0 : measured_state->twist.twist.linear.y, + std::isnan(measured_state->twist.twist.linear.z) ? 0.0 : measured_state->twist.twist.linear.z); + auto linear_vel_world = q_body_in_world * linear_vel_body; + + // if any measured angular velocity is NaN, set to zero + Eigen::Vector3d angular_vel_body( + std::isnan(measured_state->twist.twist.angular.x) ? 0.0 : measured_state->twist.twist.angular.x, + std::isnan(measured_state->twist.twist.angular.y) ? 0.0 : measured_state->twist.twist.angular.y, + std::isnan(measured_state->twist.twist.angular.z) ? 0.0 + : measured_state->twist.twist.angular.z); + auto angular_vel_world = q_body_in_world * angular_vel_body; + + state.velocities[0] = linear_vel_world[0]; + state.velocities[1] = linear_vel_world[1]; + state.velocities[2] = linear_vel_world[2]; + state.velocities[3] = angular_vel_world[0]; + state.velocities[4] = angular_vel_world[1]; + state.velocities[5] = angular_vel_world[2]; + + state.accelerations.clear(); + return true; +} + +void CartesianTrajectoryGenerator::publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, + const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, + const JointTrajectoryPoint & ruckig_input) +{ + joint_trajectory_controller::JointTrajectoryController::publish_state( + time, desired_state, current_state, state_error, splines_output, ruckig_input_target, + ruckig_input); + + if (cart_state_publisher_->trylock()) + { + cart_state_publisher_->msg_.header.stamp = time; + cart_state_publisher_->msg_.reference_world = *(*reference_world_.readFromRT()); + + auto set_multi_dof_point = + [&]( + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint & multi_dof_point, + const JointTrajectoryPoint & traj_point) + { + if (traj_point.positions.size() == 6) + { + multi_dof_point.transforms[0].translation.x = traj_point.positions[0]; + multi_dof_point.transforms[0].translation.y = traj_point.positions[1]; + multi_dof_point.transforms[0].translation.z = traj_point.positions[2]; + + std::array orientation_angles = { + traj_point.positions[3], traj_point.positions[4], traj_point.positions[5]}; + geometry_msgs::msg::Quaternion quaternion; + rpy_to_quaternion(orientation_angles, quaternion); + multi_dof_point.transforms[0].rotation = quaternion; + } + if (traj_point.velocities.size() == 6) + { + multi_dof_point.velocities[0].linear.x = traj_point.velocities[0]; + multi_dof_point.velocities[0].linear.y = traj_point.velocities[1]; + multi_dof_point.velocities[0].linear.z = traj_point.velocities[2]; + multi_dof_point.velocities[0].angular.x = traj_point.velocities[3]; + multi_dof_point.velocities[0].angular.y = traj_point.velocities[4]; + multi_dof_point.velocities[0].angular.z = traj_point.velocities[5]; + } + if (traj_point.accelerations.size() == 6) + { + multi_dof_point.accelerations[0].linear.x = traj_point.accelerations[0]; + multi_dof_point.accelerations[0].linear.y = traj_point.accelerations[1]; + multi_dof_point.accelerations[0].linear.z = traj_point.accelerations[2]; + multi_dof_point.accelerations[0].angular.x = traj_point.accelerations[3]; + multi_dof_point.accelerations[0].angular.y = traj_point.accelerations[4]; + multi_dof_point.accelerations[0].angular.z = traj_point.accelerations[5]; + } + }; + + set_multi_dof_point(cart_state_publisher_->msg_.feedback_local, current_state); + set_multi_dof_point(cart_state_publisher_->msg_.error, state_error); + set_multi_dof_point(cart_state_publisher_->msg_.output_world, desired_state); + + const auto measured_state = *(feedback_.readFromRT()); + cart_state_publisher_->msg_.feedback.transforms[0].translation.x = + measured_state->pose.pose.position.x; + cart_state_publisher_->msg_.feedback.transforms[0].translation.y = + measured_state->pose.pose.position.y; + cart_state_publisher_->msg_.feedback.transforms[0].translation.z = + measured_state->pose.pose.position.z; + cart_state_publisher_->msg_.feedback.transforms[0].rotation = + measured_state->pose.pose.orientation; + cart_state_publisher_->msg_.feedback.velocities[0] = measured_state->twist.twist; + + cart_state_publisher_->unlockAndPublish(); + } +} + +} // namespace cartesian_trajectory_generator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_trajectory_generator::CartesianTrajectoryGenerator, + controller_interface::ControllerInterface) diff --git a/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp b/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp new file mode 100644 index 0000000000..84454b57f7 --- /dev/null +++ b/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "gmock/gmock.h" + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadJointStateController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_cartesian_trajectory_generator", + "trajectory_interpolators/CartesianTrajectoryGenerator")); + + rclcpp::shutdown(); +} diff --git a/multi_time_trajectory_controller/CMakeLists.txt b/multi_time_trajectory_controller/CMakeLists.txt new file mode 100644 index 0000000000..7dd54a7b23 --- /dev/null +++ b/multi_time_trajectory_controller/CMakeLists.txt @@ -0,0 +1,117 @@ +cmake_minimum_required(VERSION 3.16) +project(multi_time_trajectory_controller LANGUAGES CXX) + +# add_compile_options(-fsanitize=address -fsanitize=undefined -fno-omit-frame-pointer) +# add_link_options(-fsanitize=address -fsanitize=undefined) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + joint_limits + joint_trajectory_controller + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rsl + tl_expected + trajectory_msgs + tf2 + tf2_geometry_msgs + urdf +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(multi_time_trajectory_controller_parameters + src/multi_time_trajectory_controller_parameters.yaml + include/multi_time_trajectory_controller/validate_mttc_parameters.hpp +) + +add_library(multi_time_trajectory_controller SHARED + src/multi_time_trajectory_controller.cpp + src/trajectory.cpp +) +target_compile_features(multi_time_trajectory_controller PUBLIC cxx_std_17) +target_include_directories(multi_time_trajectory_controller PUBLIC + $ + $ +) +target_link_libraries(multi_time_trajectory_controller PUBLIC + multi_time_trajectory_controller_parameters +) +ament_target_dependencies(multi_time_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface multi_time_plugin.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_mttc + test/test_mttc.cpp) + set_tests_properties(test_mttc PROPERTIES TIMEOUT 260) + target_link_libraries(test_mttc + multi_time_trajectory_controller + ) + target_compile_definitions(multi_time_trajectory_controller PRIVATE _USE_MATH_DEFINES) + + ament_add_gmock(test_load_mttc + test/test_load_mttc.cpp + ) + target_link_libraries(test_load_mttc + multi_time_trajectory_controller + ) + ament_target_dependencies(test_load_mttc + controller_manager + control_toolbox + realtime_tools + ros2_control_test_assets + ) + + set(BUILD_VISUAL_TESTS ON) + if(BUILD_VISUAL_TESTS) + find_package(data_tamer_cpp REQUIRED) + ament_add_gmock(mac_visual_test + test/mac_visual_test.cpp) + set_tests_properties(mac_visual_test PROPERTIES TIMEOUT 220) + target_link_libraries(mac_visual_test + multi_time_trajectory_controller + data_tamer_cpp::data_tamer + ) + ament_target_dependencies(mac_visual_test + data_tamer_cpp) + endif() +endif() + +install( + DIRECTORY include/ + DESTINATION include/multi_time_trajectory_controller +) +install(TARGETS + multi_time_trajectory_controller + multi_time_trajectory_controller_parameters + EXPORT export_multi_time_trajectory_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_multi_time_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/multi_time_trajectory_controller/include/multi_time_trajectory_controller/multi_time_trajectory_controller.hpp b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/multi_time_trajectory_controller.hpp new file mode 100644 index 0000000000..531b78530c --- /dev/null +++ b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/multi_time_trajectory_controller.hpp @@ -0,0 +1,359 @@ +// 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 MULTI_TIME_TRAJECTORY_CONTROLLER__MULTI_TIME_TRAJECTORY_CONTROLLER_HPP_ +#define MULTI_TIME_TRAJECTORY_CONTROLLER__MULTI_TIME_TRAJECTORY_CONTROLLER_HPP_ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/action/follow_axis_trajectory.hpp" +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "control_msgs/msg/multi_axis_trajectory.hpp" +#include "control_msgs/msg/multi_time_trajectory_controller_state.hpp" +#include "control_msgs/srv/query_trajectory_state.hpp" +#include "control_msgs/srv/reset_dofs.hpp" +#include "controller_interface/controller_interface.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp_action/server.hpp" +#include "tolerances.hpp" +#include "trajectory.hpp" + +// auto-generated by generate_parameter_library +#include "multi_time_trajectory_controller/multi_time_trajectory_controller_parameters.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace multi_time_trajectory_controller +{ + +control_msgs::msg::AxisTrajectoryPoint emptyTrajectoryPoint(); + +class MultiTimeTrajectoryController : public controller_interface::ControllerInterface +{ +public: + MultiTimeTrajectoryController(); + + /** + * @brief command_interface_configuration + */ + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration + */ + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + using ControllerReferenceMsg = control_msgs::msg::MultiAxisTrajectory; + using ControllerFeedbackMsg = nav_msgs::msg::Odometry; + +protected: + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, + hardware_interface::HW_IF_EFFORT, + }; + + // Preallocate variables used in the realtime update() function + std::vector state_current_; + std::vector command_current_; + std::vector state_desired_; + std::vector state_error_; + std::vector splines_state_; + + // Degrees of freedom + size_t dof_; + + // Storing command axis names for interfaces + std::vector command_axis_names_; + + // Parameters from ROS for multi_time_trajectory_controller + std::shared_ptr param_listener_; + Params params_; + + std::vector last_commanded_state_; + std::vector last_commanded_time_; + /// Specify interpolation method. Default to splines. + joint_trajectory_controller::interpolation_methods::InterpolationMethod interpolation_method_{ + joint_trajectory_controller::interpolation_methods::DEFAULT_INTERPOLATION}; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in axis_names_ + template + using AxisInterfaceRefs = std::vector>; + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences axis_command_interface_; + InterfaceReferences axis_state_interface_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + /// If true, a velocity feedforward term plus corrective PID term is used + bool use_closed_loop_pid_adapter_ = false; + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + // Configuration for every axis if it wraps around (ie. is continuous, position error is + // normalized) + std::vector axis_angle_wraparound_; + + // joint limiter configuration for JTC + std::vector joint_limits_; + + using JointLimiter = + joint_limits::JointLimiterInterface; + std::shared_ptr> joint_limiter_loader_; + std::unique_ptr joint_limiter_; + + // Timeout to consider commands old + double cmd_timeout_; + // True if holding position or repeating last trajectory point in case of success + realtime_tools::RealtimeBuffer rt_is_holding_; + // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr axis_command_subscriber_ = + nullptr; + + rclcpp::Service::SharedPtr query_state_srv_; + + std::shared_ptr traj_external_point_ptr_ = nullptr; + realtime_tools::RealtimeBuffer> + traj_msg_external_point_ptr_; + control_msgs::msg::MultiAxisTrajectory trajectory_msg_recvd_; + std::vector reset_dofs_positions_; + bool is_reliable_update_pending_ = false; + + std::shared_ptr hold_position_msg_ptr_ = nullptr; + + using FollowJTrajAction = control_msgs::action::FollowAxisTrajectory; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + rclcpp_action::Server::SharedPtr action_server_; + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + realtime_tools::RealtimeBuffer rt_has_pending_goal_; ///< Is there a pending action goal? + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + using ControllerResetDofsSrvType = control_msgs::srv::ResetDofs; + + struct ResetDofsData + { + bool reset; + double position; + double velocity; + double acceleration; + }; + realtime_tools::RealtimeBuffer> reset_dofs_flags_; + rclcpp::Service::SharedPtr reset_dofs_service_; + + using ControllerStateMsg = control_msgs::msg::MultiTimeTrajectoryControllerState; + using StatePublisher = realtime_tools::RealtimePublisher; + using StatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr publisher_; + StatePublisherPtr state_publisher_; + rclcpp::Publisher::SharedPtr splines_output_pub_; + StatePublisherPtr splines_output_publisher_; + + using TrajectoryPoint = control_msgs::msg::AxisTrajectoryPoint; + + virtual void publish_state( + const rclcpp::Time & time, const bool first_sample, const std::vector & segment_start); + + // callback for topic interface + + void topic_callback(const std::shared_ptr msg); + + // callbacks for action_server_ + + rclcpp_action::GoalResponse goal_received_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + + void goal_accepted_callback( + std::shared_ptr> goal_handle); + + /** + * Computes the error for a specific axis in the trajectory. + * + * @param[out] error The computed error for the axis. + * @param[in] index The index of the axis in the trajectory. + * @param[in] current The current state of the axes. + * @param[in] desired The desired state of the axes. + */ + + void compute_error( + std::vector & error, const std::vector & current, + const std::vector & desired) const; + + // sorts the axes of the incoming message to our local order + + void sort_to_local_axis_order( + std::shared_ptr trajectory_msg) const; + + bool validate_trajectory_msg(const control_msgs::msg::MultiAxisTrajectory & trajectory) const; + + virtual void add_new_trajectory_msg( + const std::shared_ptr & traj_msg, + const bool reliable = false); + + bool validate_trajectory_point_field( + size_t axis_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + + SegmentTolerances default_tolerances_; + + void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ + + std::shared_ptr set_hold_position(); + + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + + std::shared_ptr set_success_trajectory_point(); + + bool reset(); + + bool has_active_trajectory() const; + + virtual bool read_state_from_hardware(std::vector & state); + + /** Assign values from the command interfaces as state. + * This is only possible if command AND state interfaces exist for the same type, + * therefore needs check for both. + * @param[out] state to be filled with values from command interfaces. + * @return true if all interfaces exists and contain non-NaN values, false otherwise. + */ + bool read_state_from_command_interfaces(std::vector & state); + void read_commands_from_command_interfaces(std::vector & commands); + + void query_state_service( + const std::shared_ptr request, + std::shared_ptr response); + + // BEGIN: helper methods + template + void assign_positions_from_interface( + std::vector & trajectory_point_interface, + const AxisInterfaceRefs & axis_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index].position = axis_interface[index].get().get_value(); + } + }; + template + void assign_velocities_from_interface( + std::vector & trajectory_point_interface, + const AxisInterfaceRefs & axis_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index].velocity = axis_interface[index].get().get_value(); + } + }; + template + void assign_accelerations_from_interface( + std::vector & trajectory_point_interface, + const AxisInterfaceRefs & axis_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index].acceleration = axis_interface[index].get().get_value(); + } + }; + +private: + rclcpp::Subscription::SharedPtr feedback_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> feedback_; + void reference_callback(const std::shared_ptr msg, const bool reliable); + ControllerFeedbackMsg last_odom_feedback_; + ControllerReferenceMsg last_reference_; + ControllerReferenceMsg last_reliable_reference_; + bool current_state_initialized_{false}; + using JointTrajectoryPoint = control_msgs::msg::AxisTrajectoryPoint; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_reliable_ = nullptr; + + // for mutual exclusion of the reference callback and the update function + std::mutex mutex_; + + void update_pids(); + + bool contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type); + + bool initialize_current_state(); + + void init_hold_position_msg(); +}; +} // namespace multi_time_trajectory_controller +#endif // MULTI_TIME_TRAJECTORY_CONTROLLER__MULTI_TIME_TRAJECTORY_CONTROLLER_HPP_ diff --git a/multi_time_trajectory_controller/include/multi_time_trajectory_controller/tolerances.hpp b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/tolerances.hpp new file mode 100644 index 0000000000..4c8bcdb87a --- /dev/null +++ b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/tolerances.hpp @@ -0,0 +1,173 @@ +// Copyright (c) 2024 ros2_control Development Team +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef MULTI_TIME_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#define MULTI_TIME_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ + +#include + +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "multi_time_trajectory_controller/multi_time_trajectory_controller_parameters.hpp" + +namespace multi_time_trajectory_controller +{ +/** + * \brief Trajectory state tolerances for position, velocity and acceleration variables. + * + * A tolerance value of zero means that no tolerance will be applied for that variable. + */ +struct StateTolerances +{ + double position = 0.0; + double velocity = 0.0; + double acceleration = 0.0; +}; + +/** + * \brief Trajectory segment tolerances. + */ +struct SegmentTolerances +{ + explicit SegmentTolerances(size_t size = 0) : state_tolerance(size), goal_state_tolerance(size) {} + + /** State tolerances that apply during segment execution. */ + std::vector state_tolerance; + + /** State tolerances that apply for the goal state only.*/ + std::vector goal_state_tolerance; + + /** Extra time after the segment end time allowed to reach the goal state tolerances. */ + double goal_time_tolerance = 0.0; +}; + +/** + * \brief Populate trajectory segment tolerances using data from the ROS node. + * + * It is assumed that the following parameter structure is followed on the provided LifecycleNode. + * Unspecified parameters will take the defaults shown in the comments: + * + * \code + * constraints: + * goal_time: 1.0 # Defaults to zero + * stopped_velocity_tolerance: 0.02 # Defaults to 0.01 + * foo_joint: + * trajectory: 0.05 # Defaults to zero (ie. the tolerance is not enforced) + * goal: 0.03 # Defaults to zero (ie. the tolerance is not enforced) + * bar_joint: + * goal: 0.01 + * \endcode + * + * \param params The ROS Parameters + * \return Trajectory segment tolerances. + */ +inline SegmentTolerances get_segment_tolerances(Params const & params) +{ + auto const & constraints = params.constraints; + auto const n_joints = params.axes.size(); + + SegmentTolerances tolerances; + tolerances.goal_time_tolerance = constraints.goal_time; + + // State and goal state tolerances + tolerances.state_tolerance.resize(n_joints); + tolerances.goal_state_tolerance.resize(n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + auto const joint = params.axes[i]; + tolerances.state_tolerance[i].position = constraints.axes_map.at(joint).trajectory; + tolerances.goal_state_tolerance[i].position = constraints.axes_map.at(joint).goal; + tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; + + auto logger = rclcpp::get_logger("tolerance"); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + } + + return tolerances; +} + +/** + * \param state_error State error to check. + * \param joint_idx Joint index for the state error + * \param state_tolerance State tolerance of joint to check \p state_error against. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT + * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. + */ +inline bool check_state_tolerance_per_joint( + const std::vector & state_error, size_t joint_idx, + const StateTolerances & state_tolerance, bool show_errors = false) +{ + using std::abs; + const double error_position = state_error[joint_idx].position; + const double error_velocity = state_error[joint_idx].velocity; + const double error_acceleration = state_error[joint_idx].acceleration; + + const bool is_valid = !( + (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) || + (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) || + (state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration)); + + if (is_valid) + { + return true; + } + + if (show_errors) + { + const auto logger = rclcpp::get_logger("tolerances"); + RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx); + + if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) + { + RCLCPP_ERROR( + logger, "Position Error: %f, Position Tolerance: %f", error_position, + state_tolerance.position); + } + if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) + { + RCLCPP_ERROR( + logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity, + state_tolerance.velocity); + } + if ( + state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) + { + RCLCPP_ERROR( + logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration, + state_tolerance.acceleration); + } + } + return false; +} + +} // namespace multi_time_trajectory_controller + +#endif // MULTI_TIME_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ diff --git a/multi_time_trajectory_controller/include/multi_time_trajectory_controller/trajectory.hpp b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/trajectory.hpp new file mode 100644 index 0000000000..b1cbb90244 --- /dev/null +++ b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/trajectory.hpp @@ -0,0 +1,258 @@ +// 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 MULTI_TIME_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ +#define MULTI_TIME_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ + +#include +#include + +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "control_msgs/msg/multi_axis_trajectory.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_trajectory_controller/interpolation_methods.hpp" +#include "rclcpp/time.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace multi_time_trajectory_controller +{ +using TrajectoryPointIter = std::vector::iterator; +using TrajectoryPointConstIter = + std::vector::const_iterator; + +class Trajectory +{ +public: + Trajectory(); + + explicit Trajectory(std::shared_ptr trajectory); + + explicit Trajectory( + const rclcpp::Time & current_time, + const std::vector & current_point, + std::shared_ptr trajectory); + + /** + * Set the point before the trajectory message is replaced/appended + * Example: if we receive a new trajectory message and it's first point is 0.5 seconds + * from the current one, we call this function to log the current state, then + * append/replace the current trajectory + * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that + * wrap around (ie. is continuous). + */ + + void set_point_before_trajectory_msg( + const std::vector & current_time, + const std::vector & current_point, + const std::vector & joints_angle_wraparound = std::vector()); + + void update( + std::shared_ptr axis_trajectory, + const std::vector & joint_limits, const rclcpp::Duration & period, + rclcpp::Time const & time); + + /// Find the segment (made up of 2 points) and its expected state from the + /// containing trajectory. + /** + * Sampling trajectory at given \p sample_time. + * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or + * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to + * be reached at the time defined in the segment. + * + * Specific case returns for start_segment_itr and end_segment_itr: + * - Sampling before the trajectory start: + * start_segment_itr = begin(), end_segment_itr = begin() + * - Sampling exactly on a point of the trajectory: + * start_segment_itr = iterator where point is, end_segment_itr = iterator after + * start_segment_itr + * - Sampling between points: + * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after + * start_segment_itr + * - Sampling after entire trajectory: + * start_segment_itr = --end(), end_segment_itr = end() + * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() + * return false + * + * \param[in] sample_time Time at which trajectory will be sampled. + * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at + * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr + * Iterator to the start segment for given \p sample_time. See description above. \param[out] + * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + */ + + std::vector sample( + const rclcpp::Time & sample_time, + const joint_trajectory_controller::interpolation_methods::InterpolationMethod + interpolation_method, + std::vector & output_state, + std::vector & start_segment_itr, + std::vector & end_segment_itr, const rclcpp::Duration & period, + std::unique_ptr< + joint_limits::JointLimiterInterface> & + joint_limiter, + std::vector & splines_state, bool hold_last_velocity); + + /** + * Do interpolation between 2 states given a time in between their respective timestamps + * + * The start and end states need not necessarily be specified all the way to the acceleration + * level: + * - If only \b positions are specified, linear interpolation will be used. + * - If \b positions and \b velocities are specified, a cubic spline will be used. + * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be + * used. + * + * If start and end states have different specifications + * (eg. start is position-only, end is position-velocity), the lowest common specification will be + * used (position-only in the example). + * + * \param[in] time_a Time at which the segment state equals \p state_a. + * \param[in] state_a State at \p time_a. + * \param[in] time_b Time at which the segment state equals \p state_b. + * \param[in] state_b State at time \p time_b. + * \param[in] sample_time The time to sample, between time_a and time_b. + * \param[out] output The state at \p sample_time. + */ + + bool interpolate_between_points( + const rclcpp::Time & time_a, const control_msgs::msg::AxisTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const control_msgs::msg::AxisTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, const bool skip_splines, + control_msgs::msg::AxisTrajectoryPoint & output, const rclcpp::Duration & period, + control_msgs::msg::AxisTrajectoryPoint & splines_state, std::size_t axis_index); + + void reset_previous_state( + const size_t axis_index, const control_msgs::msg::AxisTrajectoryPoint & state) + { + previous_state_[axis_index] = state; + } + + TrajectoryPointConstIter begin(std::size_t) const; + + TrajectoryPointConstIter end(std::size_t) const; + + rclcpp::Time start_time() const; + + std::vector const & time_before_trajectory() const { return time_before_traj_msg_; } + const std::vector & state_before_trajectory() const + { + return state_before_traj_msg_; + } + + const std::vector & state_after_interp() const + { + return output_state_after_interp_; + } + + auto interpolation_state_a() const { return interpolation_state_a_; } + + auto interpolation_state_b() const { return interpolation_state_b_; } + + auto interpoland_time_ns() const { return interpoland_time_ns_; } + + auto inter_point_time_ns() const { return inter_point_time_ns_; } + + const std::vector & state_after_joint_limit() const + { + return output_state_after_joint_limit_; + } + + const std::vector & previous_state() const + { + return previous_state_cached_; + } + + bool has_trajectory_msg() const; + + bool has_nontrivial_msg(std::size_t) const; + + std::shared_ptr get_trajectory_msg() const + { + return trajectory_msg_; + } + + bool is_sampled_already() const { return sampled_already_; } + +private: + void deduce_from_derivatives( + control_msgs::msg::AxisTrajectoryPoint & first_state, + control_msgs::msg::AxisTrajectoryPoint & second_state, const double delta_t); + + std::shared_ptr trajectory_msg_; + rclcpp::Time trajectory_start_time_; + + std::vector time_before_traj_msg_; + std::vector state_before_traj_msg_; + + bool sampled_already_ = false; + + // for logging + std::vector previous_state_; + std::vector previous_state_cached_; + std::vector inter_point_time_ns_; + std::vector interpoland_time_ns_; + std::vector output_state_after_interp_; + std::vector output_state_after_joint_limit_; + std::vector interpolation_state_a_; + std::vector interpolation_state_b_; +}; + +/** + * \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., + * the first trajectory point + * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that + * wrap around (ie. is continuous). + */ +void wraparound_joint( + const std::vector & current_position, + std::vector & next_position, + const std::vector & joints_angle_wraparound); + +} // namespace multi_time_trajectory_controller + +#endif // MULTI_TIME_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/multi_time_trajectory_controller/include/multi_time_trajectory_controller/validate_mttc_parameters.hpp b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/validate_mttc_parameters.hpp new file mode 100644 index 0000000000..ecea475111 --- /dev/null +++ b/multi_time_trajectory_controller/include/multi_time_trajectory_controller/validate_mttc_parameters.hpp @@ -0,0 +1,99 @@ +// 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 MULTI_TIME_TRAJECTORY_CONTROLLER__VALIDATE_MTTC_PARAMETERS_HPP_ +#define MULTI_TIME_TRAJECTORY_CONTROLLER__VALIDATE_MTTC_PARAMETERS_HPP_ + +#include +#include + +#include "rclcpp/parameter.hpp" +#include "rsl/algorithm.hpp" +#include "tl_expected/expected.hpp" + +namespace multi_time_trajectory_controller +{ +tl::expected command_interface_type_combinations( + rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 2. position [velocity, [acceleration]] + + if ( + rsl::contains>(interface_types, "velocity") && + interface_types.size() > 1 && + !rsl::contains>(interface_types, "position")) + { + return tl::make_unexpected( + "'velocity' command interface can be used either alone or 'position' " + "command interface has to be present"); + } + + if ( + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position"))) + { + return tl::make_unexpected( + "'acceleration' command interface can only be used if 'velocity' and " + "'position' command interfaces are present"); + } + + if ( + rsl::contains>(interface_types, "effort") && + interface_types.size() > 1) + { + return tl::make_unexpected("'effort' command interface has to be used alone"); + } + + return {}; +} + +tl::expected state_interface_type_combinations( + rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Valid combinations are + // 1. position [velocity, [acceleration]] + + if ( + rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position")) + { + return tl::make_unexpected( + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + } + + if ( + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "position") || + !rsl::contains>(interface_types, "velocity"))) + { + return tl::make_unexpected( + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + } + + return {}; +} + +} // namespace multi_time_trajectory_controller + +#endif // MULTI_TIME_TRAJECTORY_CONTROLLER__VALIDATE_MTTC_PARAMETERS_HPP_ diff --git a/multi_time_trajectory_controller/multi_time_plugin.xml b/multi_time_trajectory_controller/multi_time_plugin.xml new file mode 100644 index 0000000000..6b215ef21d --- /dev/null +++ b/multi_time_trajectory_controller/multi_time_plugin.xml @@ -0,0 +1,7 @@ + + + + The multi-time trajectory controller executes axis-space or cartesian trajectories on a set of axes. + + + diff --git a/multi_time_trajectory_controller/package.xml b/multi_time_trajectory_controller/package.xml new file mode 100644 index 0000000000..542daf926f --- /dev/null +++ b/multi_time_trajectory_controller/package.xml @@ -0,0 +1,44 @@ + + + multi_time_trajectory_controller + 4.7.0 + Controller for executing joint-space trajectories on a group of joints + Bence Magyar + Dr. Denis Štogl + Christoph Froehlich + + Apache License 2.0 + + ament_cmake + + angles + backward_ros + controller_interface + control_msgs + control_toolbox + generate_parameter_library + hardware_interface + joint_limits + joint_trajectory_controller + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rsl + tl_expected + trajectory_msgs + tf2 + tf2_geometry_msgs + urdf + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + data_tamer_cpp + + + ament_cmake + + diff --git a/multi_time_trajectory_controller/src/multi_time_trajectory_controller.cpp b/multi_time_trajectory_controller/src/multi_time_trajectory_controller.cpp new file mode 100644 index 0000000000..d25393b675 --- /dev/null +++ b/multi_time_trajectory_controller/src/multi_time_trajectory_controller.cpp @@ -0,0 +1,1957 @@ +// 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 "multi_time_trajectory_controller/multi_time_trajectory_controller.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "control_msgs/msg/axis_trajectory.hpp" +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "control_msgs/msg/multi_axis_trajectory.hpp" +#include "controller_interface/helpers.hpp" +#include "eigen3/Eigen/Eigen" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_action/create_server.hpp" + +namespace multi_time_trajectory_controller +{ + +control_msgs::msg::AxisTrajectoryPoint emptyTrajectoryPoint() +{ + control_msgs::msg::AxisTrajectoryPoint atp; + atp.position = std::numeric_limits::quiet_NaN(); + atp.velocity = std::numeric_limits::quiet_NaN(); + atp.acceleration = std::numeric_limits::quiet_NaN(); + atp.effort = std::numeric_limits::quiet_NaN(); + atp.time_from_start = rclcpp::Duration(0, 0); + return atp; +} + +MultiTimeTrajectoryController::MultiTimeTrajectoryController() +: controller_interface::ControllerInterface(), dof_(0) +{ +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_init() +{ + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + joint_limiter_loader_ = std::make_shared>( + "joint_limits", "joint_limits::JointLimiterInterface"); + RCLCPP_DEBUG(get_node()->get_logger(), "Available joint limiter classes:"); + for (const auto & available_class : joint_limiter_loader_->getDeclaredClasses()) + { + RCLCPP_DEBUG(get_node()->get_logger(), " %s", available_class.c_str()); + } + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + axis_angle_wraparound_ = params_.axes_is_angular; + + const std::string & urdf = get_robot_description(); + if (!urdf.empty()) + { + urdf::Model model; + if (!model.initString(urdf)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse robot description!"); + return CallbackReturn::ERROR; + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); + } + } + else + { + // empty URDF is used for some tests + RCLCPP_ERROR(get_node()->get_logger(), "No URDF file given"); + } + + // Initialize joint limits + joint_limits_.resize(params_.axes.size()); + + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +MultiTimeTrajectoryController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + if (dof_ == 0) + { + fprintf( + stderr, + "During ros2_control interface configuration, degrees of freedom is not valid;" + " it should be positive. Actual DOF is %zu\n", + dof_); + std::exit(EXIT_FAILURE); + } + conf.names.reserve(dof_ * params_.command_interfaces.size()); + for (const auto & axis_name : command_axis_names_) + { + for (const auto & interface_type : params_.command_interfaces) + { + conf.names.push_back(axis_name + "/" + interface_type); + } + } + return conf; +} + +controller_interface::InterfaceConfiguration +MultiTimeTrajectoryController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + if (params_.state_interfaces.size() == 0) + { + conf.type = controller_interface::interface_configuration_type::NONE; + return conf; + } + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(dof_ * params_.state_interfaces.size()); + for (const auto & axis_name : params_.axes) + { + for (const auto & interface_type : params_.state_interfaces) + { + conf.names.push_back(axis_name + "/" + interface_type); + } + } + return conf; +} + +controller_interface::return_type MultiTimeTrajectoryController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + return controller_interface::return_type::OK; + } + + // bail if haven't initialized current state and still can't + if (!current_state_initialized_ && !initialize_current_state()) + { + return controller_interface::return_type::OK; + } + + // update dynamic parameters + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + default_tolerances_ = get_segment_tolerances(params_); + // update the PID gains + // variable use_closed_loop_pid_adapter_ is updated in on_configure only + if (use_closed_loop_pid_adapter_) + { + update_pids(); + } + } + if (is_reliable_update_pending_) is_reliable_update_pending_ = false; + + // don't update goal after we sampled the trajectory to avoid any racecondition + const auto active_goal = *rt_active_goal_.readFromRT(); + + // Check if a new external message has been received from nonRT threads + { + std::lock_guard guard(mutex_); + auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); + auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + if ( + current_external_msg != *new_external_msg && + (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) + { + sort_to_local_axis_order(*new_external_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(*new_external_msg, joint_limits_, period, time); + } + } + + // current state update - bail if can't read hardware state + if (!read_state_from_hardware(state_current_)) return controller_interface::return_type::OK; + + bool first_sample = false; + std::vector segment_start(dof_, -1); + std::fill( + reset_dofs_positions_.begin(), reset_dofs_positions_.end(), + std::numeric_limits::quiet_NaN()); + + // currently carrying out a trajectory + if (has_active_trajectory()) + { + // if sampling the first time, set the point before you sample + if (!traj_external_point_ptr_->is_sampled_already()) + { + first_sample = true; + + if (params_.open_loop_control) + { + auto reset_flags = reset_dofs_flags_.readFromRT(); + for (size_t i = 0; i < dof_; ++i) + { + if (reset_flags->at(i).reset) + { + // if we reset, treat it as the last command + last_commanded_time_[i] = time; + last_commanded_state_[i].position = std::isnan(reset_flags->at(i).position) + ? state_current_[i].position + : reset_flags->at(i).position; + reset_dofs_positions_[i] = last_commanded_state_[i].position; + RCLCPP_INFO_STREAM( + get_node()->get_logger(), command_axis_names_[i] + << ": last commanded state position reset to " + << last_commanded_state_[i].position); + if (has_velocity_state_interface_) + { + last_commanded_state_[i].velocity = std::isnan(reset_flags->at(i).velocity) + ? state_current_[i].velocity + : reset_flags->at(i).velocity; + } + if (has_acceleration_state_interface_) + { + last_commanded_state_[i].acceleration = std::isnan(reset_flags->at(i).acceleration) + ? state_current_[i].acceleration + : reset_flags->at(i).acceleration; + } + + reset_flags->at(i).reset = false; // reset flag in the buffer for one-shot execution + + // reset previous state for the trajectory to new internal state + // position is set from reset command and we reset velocity/accel here + // time is not used so we don't need to set it + auto prev_state = last_commanded_state_[i]; + prev_state.velocity = prev_state.acceleration = 0; + traj_external_point_ptr_->reset_previous_state(i, prev_state); + } + } + + // TODO(bijoua29): change below implementation + // if all of the velocities at the last commanded state are zero, then we should have held + // that position the whole time so set the last commanded time to now so we don't + // interpolate from whenever the last trajectory ended + for (std::size_t axis_index = 0; axis_index < dof_; ++axis_index) + { + auto const & vel = last_commanded_state_[axis_index].velocity; + if (vel == 0 || std::isnan(vel)) + { + last_commanded_time_[axis_index] = time; + } + + if (last_commanded_time_[axis_index].seconds() == 0.0) + { + last_commanded_time_[axis_index] = time - period; + } + } + traj_external_point_ptr_->set_point_before_trajectory_msg( + last_commanded_time_, last_commanded_state_, axis_angle_wraparound_); + } + else + { + traj_external_point_ptr_->set_point_before_trajectory_msg( + std::vector{dof_, time}, state_current_, axis_angle_wraparound_); + } + } + + // find segment for current timestamp + std::vector start_segment_itrs, end_segment_itrs; + start_segment_itrs.resize(dof_); + end_segment_itrs.resize(dof_); + auto const valid_points = traj_external_point_ptr_->sample( + time, interpolation_method_, state_desired_, start_segment_itrs, end_segment_itrs, period, + joint_limiter_, splines_state_, params_.hold_last_velocity); + + compute_error(state_error_, state_current_, state_desired_); + + for (std::size_t axis_index = 0; axis_index < dof_; ++axis_index) + { + if (valid_points[axis_index]) + { + auto const & start_segment_itr = start_segment_itrs[axis_index]; + auto const & end_segment_itr = end_segment_itrs[axis_index]; + const rclcpp::Time traj_start = traj_external_point_ptr_->start_time(); + + if ( + (start_segment_itr == end_segment_itr) && + (std::distance(traj_external_point_ptr_->begin(axis_index), start_segment_itr) == 0)) + { + segment_start[axis_index] = -1; + } + else + { + segment_start[axis_index] = static_cast( + std::distance(traj_external_point_ptr_->begin(axis_index), start_segment_itr)); + } + + // this is the time instance + // - started with the first segment: when the first point will be reached (in the future) + // - later: when the point of the current segment was reached + const rclcpp::Time segment_time_from_start = + traj_start + start_segment_itr->time_from_start; + // time_difference is + // - negative until first point is reached + // - counting from zero to time_from_start of next point + double time_difference = time.seconds() - segment_time_from_start.seconds(); + bool tolerance_violated_while_moving = false; + bool outside_goal_tolerance = false; + bool within_goal_time = true; + + // whether we are before the last point for the trajectory associated with axis_index + bool const before_last_point = end_segment_itr != traj_external_point_ptr_->end(axis_index); + + // have we reached the end, are not holding position, and is a timeout configured? + // Check independently of other tolerances + if ( + !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && + time_difference > cmd_timeout_) + { + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + + // Check state/goal tolerance + // Always check the state tolerance on the first sample in case the first sample + // is the last point + // print output per default, goal will be aborted afterwards + if ( + (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && + !check_state_tolerance_per_joint( + state_error_, axis_index, default_tolerances_.state_tolerance[axis_index], + true /* show_errors */)) + { + tolerance_violated_while_moving = true; + } + // past the final point, check that we end up inside goal tolerance + if ( + !before_last_point && *(rt_is_holding_.readFromRT()) == false && + !check_state_tolerance_per_joint( + state_error_, axis_index, default_tolerances_.goal_state_tolerance[axis_index], + false /* show_errors */)) + { + outside_goal_tolerance = true; + + if (default_tolerances_.goal_time_tolerance != 0.0) + { + if (time_difference > default_tolerances_.goal_time_tolerance) + { + within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + state_error_, axis_index, default_tolerances_.goal_state_tolerance[axis_index], + true /* show_errors */); + } + } + } + + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + double tmp_command = 0; + if (use_closed_loop_pid_adapter_) + { + // Update PID + tmp_command = + (state_desired_[axis_index].velocity * ff_velocity_scale_[axis_index]) + + pids_[axis_index]->compute_command( + state_error_[axis_index].position, state_error_[axis_index].velocity, period); + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + if (!axis_command_interface_[0][axis_index].get().set_value( + state_desired_[axis_index].position)) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set position command for " << command_axis_names_[axis_index]); + } + } + if (has_velocity_command_interface_) + { + bool ret_status; + if (use_closed_loop_pid_adapter_) + { + ret_status = axis_command_interface_[1][axis_index].get().set_value(tmp_command); + } + else + { + ret_status = axis_command_interface_[1][axis_index].get().set_value( + state_desired_[axis_index].velocity); + } + if (!ret_status) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set velocity command for " << command_axis_names_[axis_index]); + } + } + if (has_acceleration_command_interface_) + { + if (!axis_command_interface_[2][axis_index].get().set_value( + state_desired_[axis_index].acceleration)) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set acceleration command for " << command_axis_names_[axis_index]); + } + } + if (has_effort_command_interface_) + { + if (!axis_command_interface_[3][axis_index].get().set_value(tmp_command)) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set effort command for " << command_axis_names_[axis_index]); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_[axis_index] = state_desired_[axis_index]; + last_commanded_time_[axis_index] = time; + } + + if (active_goal) + { + // send feedback + auto feedback = std::make_shared(); + feedback->header.stamp = time; + feedback->axis_names = params_.axes; + + feedback->actual_points = state_current_; + feedback->desired_points = state_desired_; + feedback->errors = state_error_; + active_goal->setFeedback(feedback); + + // check abort + if (tolerance_violated_while_moving) + { + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); + + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + // check goal tolerance + else if (!before_last_point) + { + if (!outside_goal_tolerance) + { + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); + + RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); + } + else if (!within_goal_time) + { + const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " + + std::to_string(time_difference) + " seconds"; + + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); + + RCLCPP_WARN(get_node()->get_logger(), "%s", error_string.c_str()); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + } + } + else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) + { + // we need to ensure that there is no pending goal -> we get a race condition otherwise + RCLCPP_ERROR( + get_node()->get_logger(), "Holding position due to state tolerance violation"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + else if ( + !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied (will stay in this state until new message arrives) + // or outside_goal_tolerance violated within the goal_time_tolerance + } + } + } + + publish_state(time, first_sample, segment_start); + return controller_interface::return_type::OK; +} + +bool MultiTimeTrajectoryController::read_state_from_hardware(std::vector & states) +{ + // start by emptying all states + std::fill(states.begin(), states.end(), emptyTrajectoryPoint()); + + if (params_.use_feedback) + { + if (dof_ != 6) + { + throw std::runtime_error("must have 6 degrees of freedom to use feedback"); + } + std::array orientation_angles; + const auto measured_state = *(feedback_.readFromRT()); + if (!measured_state) return false; + last_odom_feedback_ = *measured_state; + + tf2::Quaternion measured_q; + + if ( + std::isnan(measured_state->pose.pose.orientation.w) || + std::isnan(measured_state->pose.pose.orientation.x) || + std::isnan(measured_state->pose.pose.orientation.y) || + std::isnan(measured_state->pose.pose.orientation.z)) + { + // if any of the orientation is NaN, revert to previous orientation + measured_q.setRPY(states[3].position, states[4].position, states[5].position); + } + else + { + tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); + } + tf2::Matrix3x3 m(measured_q); + m.getRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + + // Assign values from the hardware + // Position states always exist + // if any measured position is NaN, keep previous value + states[0].position = std::isnan(measured_state->pose.pose.position.x) + ? states[0].position + : measured_state->pose.pose.position.x; + states[1].position = std::isnan(measured_state->pose.pose.position.y) + ? states[1].position + : measured_state->pose.pose.position.y; + states[2].position = std::isnan(measured_state->pose.pose.position.z) + ? states[2].position + : measured_state->pose.pose.position.z; + states[3].position = orientation_angles[0]; + states[4].position = orientation_angles[1]; + states[5].position = orientation_angles[2]; + + // Convert measured twist which is in body frame to world frame since CTG/JTC expects state + // in world frame + + Eigen::Quaterniond q_body_in_world( + measured_q.w(), measured_q.x(), measured_q.y(), measured_q.z()); + + // if any measured linear velocity is NaN, set to zero + Eigen::Vector3d linear_vel_body( + std::isnan(measured_state->twist.twist.linear.x) ? 0.0 : measured_state->twist.twist.linear.x, + std::isnan(measured_state->twist.twist.linear.y) ? 0.0 : measured_state->twist.twist.linear.y, + std::isnan(measured_state->twist.twist.linear.z) ? 0.0 + : measured_state->twist.twist.linear.z); + auto linear_vel_world = q_body_in_world * linear_vel_body; + + // if any measured angular velocity is NaN, set to zero + Eigen::Vector3d angular_vel_body( + std::isnan(measured_state->twist.twist.angular.x) ? 0.0 + : measured_state->twist.twist.angular.x, + std::isnan(measured_state->twist.twist.angular.y) ? 0.0 + : measured_state->twist.twist.angular.y, + std::isnan(measured_state->twist.twist.angular.z) ? 0.0 + : measured_state->twist.twist.angular.z); + auto angular_vel_world = q_body_in_world * angular_vel_body; + + states[0].velocity = linear_vel_world[0]; + states[1].velocity = linear_vel_world[1]; + states[2].velocity = linear_vel_world[2]; + states[3].velocity = angular_vel_world[0]; + states[4].velocity = angular_vel_world[1]; + states[5].velocity = angular_vel_world[2]; + + for (std::size_t i = 0; i < 6; ++i) + { + states[i].acceleration = std::numeric_limits::quiet_NaN(); + } + return true; + } + else + { + // Assign values from the hardware + if (has_position_state_interface_) + { + assign_positions_from_interface(states, axis_state_interface_[0]); + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + assign_velocities_from_interface(states, axis_state_interface_[1]); + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) + { + assign_accelerations_from_interface(states, axis_state_interface_[2]); + } + } + } + } + return true; +} + +bool MultiTimeTrajectoryController::read_state_from_command_interfaces( + std::vector & states) +{ + // start by emptying all states + std::fill(states.begin(), states.end(), emptyTrajectoryPoint()); + bool has_values = true; + + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + }; + + // Assign values from the command interfaces as state + if (has_position_command_interface_ && interface_has_values(axis_command_interface_[0])) + { + assign_positions_from_interface(states, axis_command_interface_[0]); + } + else + { + has_values = false; + } + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + if (has_velocity_command_interface_ && interface_has_values(axis_command_interface_[1])) + { + assign_velocities_from_interface(states, axis_command_interface_[1]); + } + else + { + has_values = false; + } + } + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) + { + if (has_acceleration_command_interface_ && interface_has_values(axis_command_interface_[2])) + { + assign_accelerations_from_interface(states, axis_command_interface_[2]); + } + else + { + has_values = false; + } + } + + return has_values; +} + +void MultiTimeTrajectoryController::read_commands_from_command_interfaces( + std::vector & commands) +{ + // start by emptying all commands + std::fill(commands.begin(), commands.end(), emptyTrajectoryPoint()); + + // Assign values from the command interfaces as command. + if (has_position_command_interface_) + { + assign_positions_from_interface(commands, axis_command_interface_[0]); + } + if (has_velocity_command_interface_) + { + assign_velocities_from_interface(commands, axis_command_interface_[1]); + } + if (has_acceleration_command_interface_) + { + assign_accelerations_from_interface(commands, axis_command_interface_[2]); + } +} + +void MultiTimeTrajectoryController::query_state_service( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto logger = get_node()->get_logger(); + // Preconditions + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); + response->success = false; + return; + } + const auto active_goal = *rt_active_goal_.readFromRT(); + response->name = params_.axes; + std::vector state_requested = state_current_; + if (has_active_trajectory()) + { + std::vector start_segment_itrs, end_segment_itrs; + start_segment_itrs.resize(dof_); + end_segment_itrs.resize(dof_); + const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); + auto const valid_points = traj_external_point_ptr_->sample( + static_cast(request->time), interpolation_method_, state_requested, + start_segment_itrs, end_segment_itrs, period, joint_limiter_, splines_state_, + params_.hold_last_velocity); + response->success = + std::all_of(valid_points.begin(), valid_points.end(), [](bool b) { return b; }); + // If the requested sample time precedes the trajectory finish time respond as failure + if (response->success) + { + bool any_is_end = false; + for (std::size_t i = 0; i < dof_; ++i) + { + if (end_segment_itrs[i] == traj_external_point_ptr_->end(i)) + { + any_is_end = true; + } + } + if (any_is_end) + { + RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); + response->success = false; + return; + } + } + else + { + RCLCPP_ERROR( + logger, "Requested sample time is earlier than the current trajectory start time."); + } + } + else + { + RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance."); + response->success = false; + return; + } + response->position.clear(); + response->velocity.clear(); + response->acceleration.clear(); + for (std::size_t i = 0; i < dof_; ++i) + { + response->position.push_back(state_requested[i].position); + response->velocity.push_back(state_requested[i].velocity); + response->acceleration.push_back(state_requested[i].acceleration); + } +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not get param listener during configure"); + return controller_interface::CallbackReturn::ERROR; + } + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // get degrees of freedom + dof_ = params_.axes.size(); + + // size all state vectors appropriately + state_current_.resize(dof_, emptyTrajectoryPoint()); + command_current_.resize(dof_, emptyTrajectoryPoint()); + state_desired_.resize(dof_, emptyTrajectoryPoint()); + state_error_.resize(dof_, emptyTrajectoryPoint()); + splines_state_.resize(dof_, emptyTrajectoryPoint()); + axis_angle_wraparound_.resize(dof_, false); + reset_dofs_positions_.resize(dof_, std::numeric_limits::quiet_NaN()); + + // TODO(destogl): why is this here? Add comment or move + if (!reset()) + { + return CallbackReturn::FAILURE; + } + + if (params_.axes.empty()) + { + RCLCPP_ERROR(logger, "'axes' parameter is empty."); + return CallbackReturn::FAILURE; + } + + command_axis_names_ = params_.command_axes; + + if (command_axis_names_.empty()) + { + command_axis_names_ = params_.axes; + RCLCPP_INFO( + logger, "No specific axis names are used for command interfaces. Using 'axes' parameter."); + } + else if (command_axis_names_.size() != params_.axes.size()) + { + RCLCPP_ERROR( + logger, "'command_axis_names' parameter has to have the same size as 'axes' parameter."); + return CallbackReturn::FAILURE; + } + + if (params_.axes_is_angular.size() != dof_) + { + RCLCPP_ERROR( + logger, "'axes_is_angular' parameter has to have the same size as 'axes' parameter."); + return CallbackReturn::FAILURE; + } + + if (params_.command_interfaces.empty()) + { + RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); + return CallbackReturn::FAILURE; + } + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + axis_command_interface_.resize(allowed_interface_types_.size()); + + has_position_command_interface_ = + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); + + // then use also PID adapter + use_closed_loop_pid_adapter_ = + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + !params_.open_loop_control) || + has_effort_command_interface_; + + if (use_closed_loop_pid_adapter_) + { + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + + update_pids(); + } + + // Initialize joint limits + if (!params_.joint_limiter_type.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), "Using joint limiter plugin: '%s'", + params_.joint_limiter_type.c_str()); + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_->createUnmanagedInstance(params_.joint_limiter_type)); + joint_limiter_->init(command_axis_names_, get_node()); + } + else + { + RCLCPP_INFO(get_node()->get_logger(), "Not using joint limiter plugin as none defined."); + } + + if (params_.state_interfaces.empty() && !params_.use_feedback) + { + RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty and not using feedback."); + return CallbackReturn::FAILURE; + } + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + // Note: 'effort' storage is also here, but never used. Still, for this is OK. + axis_state_interface_.resize(allowed_interface_types_.size()); + + has_position_state_interface_ = + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + + // Validation of combinations of state and velocity together have to be done + // here because the parameter validators only deal with each parameter + // separately. + if ( + has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + (!has_velocity_state_interface_ || !has_position_state_interface_)) + { + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } + + // effort is always used alone so no need for size check + if ( + has_effort_command_interface_ && + (!has_velocity_state_interface_ || !has_position_state_interface_)) + { + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } + + auto get_interface_list = [](const std::vector & interface_types) + { + std::stringstream ss_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) + { + if (index != 0) + { + ss_interfaces << " "; + } + ss_interfaces << interface_types[index]; + } + return ss_interfaces.str(); + }; + + // Print output so users can be sure the interface setup is correct + RCLCPP_INFO( + logger, "Command interfaces are [%s] and state interfaces are [%s].", + get_interface_list(params_.command_interfaces).c_str(), + get_interface_list(params_.state_interfaces).c_str()); + + // parse remaining parameters + const std::string interpolation_string = + get_node()->get_parameter("interpolation_method").as_string(); + interpolation_method_ = + joint_trajectory_controller::interpolation_methods::from_string(interpolation_string); + RCLCPP_INFO( + logger, "Using '%s' interpolation method.", + joint_trajectory_controller::interpolation_methods::InterpolationMethodMap + .at(interpolation_method_) + .c_str()); + + // prepare hold_position_msg + init_hold_position_msg(); + + // create subscriber and publishers + axis_command_subscriber_ = + get_node()->create_subscription( + "~/axis_trajectory", rclcpp::SystemDefaultsQoS(), + std::bind(&MultiTimeTrajectoryController::topic_callback, this, std::placeholders::_1)); + + publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(publisher_); + + state_publisher_->lock(); + state_publisher_->msg_.axis_names = params_.axes; + state_publisher_->msg_.feedback.resize(dof_); + state_publisher_->msg_.error.resize(dof_); + state_publisher_->msg_.output.resize(dof_); + state_publisher_->msg_.state_before_trajectory.resize(dof_); + state_publisher_->msg_.state_after_interpolation.resize(dof_); + state_publisher_->msg_.state_after_joint_limit.resize(dof_); + state_publisher_->msg_.segment_start.resize(dof_); + state_publisher_->msg_.reset_dofs_position.resize(dof_); + state_publisher_->unlock(); + + splines_output_pub_ = get_node()->create_publisher( + "~/splines_output", rclcpp::SystemDefaultsQoS()); + splines_output_publisher_ = std::make_unique(splines_output_pub_); + + splines_output_publisher_->lock(); + splines_output_publisher_->msg_.axis_names = command_axis_names_; + splines_output_publisher_->msg_.feedback.resize(dof_); + splines_output_publisher_->msg_.error.resize(dof_); + splines_output_publisher_->msg_.output.resize(dof_); + splines_output_publisher_->msg_.state_before_trajectory.resize(dof_); + splines_output_publisher_->msg_.state_after_interpolation.resize(dof_); + splines_output_publisher_->msg_.state_after_joint_limit.resize(dof_); + splines_output_publisher_->msg_.segment_start.resize(dof_); + splines_output_publisher_->msg_.reset_dofs_position.resize(dof_); + splines_output_publisher_->unlock(); + + RCLCPP_INFO( + logger, "Action status changes will be monitored at %.2f Hz.", params_.action_monitor_rate); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); + + using namespace std::placeholders; + action_server_ = rclcpp_action::create_server( + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string(get_node()->get_name()) + "/follow_joint_trajectory", + std::bind(&MultiTimeTrajectoryController::goal_received_callback, this, _1, _2), + std::bind(&MultiTimeTrajectoryController::goal_cancelled_callback, this, _1), + std::bind(&MultiTimeTrajectoryController::goal_accepted_callback, this, _1)); + + query_state_srv_ = get_node()->create_service( + std::string(get_node()->get_name()) + "/query_state", + std::bind(&MultiTimeTrajectoryController::query_state_service, this, _1, _2)); + + std::vector reset_flags; + reset_flags.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + reset_dofs_flags_.writeFromNonRT(reset_flags); + + // topics QoS + auto qos_best_effort_history_depth_one = rclcpp::SystemDefaultsQoS(); + qos_best_effort_history_depth_one.keep_last(1); + qos_best_effort_history_depth_one.best_effort(); + auto subscribers_reliable_qos = rclcpp::SystemDefaultsQoS(); + subscribers_reliable_qos.keep_all(); + subscribers_reliable_qos.reliable(); + + // Reference Subscribers (reliable channel also for updates not to be missed) + // see https://answers.ros.org/question/308386/ros2-add-arguments-to-callback/ for why we create a + // bound function when we need to pass in additional arguments + std::function bound_ref_func = std::bind( + &MultiTimeTrajectoryController::reference_callback, this, std::placeholders::_1, false); + ref_subscriber_ = get_node()->create_subscription( + "~/reference", qos_best_effort_history_depth_one, bound_ref_func); + std::function bound_ref_rel_func = std::bind( + &MultiTimeTrajectoryController::reference_callback, this, std::placeholders::_1, true); + ref_subscriber_reliable_ = get_node()->create_subscription( + "~/reference_reliable", subscribers_reliable_qos, bound_ref_rel_func); + + std::shared_ptr msg = std::make_shared(); + msg->axis_names = params_.axes; + msg->axis_trajectories.resize(dof_); + for (std::size_t i = 0; i < dof_; ++i) + { + msg->axis_trajectories[i].axis_points.resize(1, emptyTrajectoryPoint()); + } + + // Odometry feedback + auto feedback_callback = [&](const std::shared_ptr feedback_msg) -> void + { feedback_.writeFromNonRT(feedback_msg); }; + feedback_subscriber_ = get_node()->create_subscription( + "~/feedback", qos_best_effort_history_depth_one, feedback_callback); + // initialize feedback to null pointer since it is used to determine if we have valid data or not + feedback_.writeFromNonRT(nullptr); + + // Control mode service + auto reset_dofs_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + response->ok = true; + + if ( + (request->positions.size() != request->velocities.size()) || + (request->velocities.size() != request->accelerations.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has different values size for positions %ld, velocities %ld, " + "accelerations %ld", + request->positions.size(), request->velocities.size(), request->accelerations.size()); + response->ok = false; + return; + } + + if ((request->positions.size() > 0) && (request->names.size() != request->positions.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has names size %ld different than positions size %ld", + request->names.size(), request->positions.size()); + response->ok = false; + return; + } + + std::vector reset_flags_reset; + reset_flags_reset.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + + // Here we read current reset dofs flags and clear it. This is done so we can add this new + // request to the existing reset flags. This logic prevents this new request from overwriting + // any previous request that hasn't been processed yet. The one assumption made here is that the + // current reset flags are not going to be processed between the two calls here to read and + // reset, which is a highly unlikely scenario. Even if it was, the behavior is fairly benign in + // that the dofs in the previous request will be reset twice. + auto reset_flags_local = *reset_dofs_flags_.readFromNonRT(); + reset_dofs_flags_.writeFromNonRT(reset_flags_reset); + + // add/update reset dofs flags from request + for (size_t i = 0; i < request->names.size(); ++i) + { + auto it = + std::find(command_axis_names_.begin(), command_axis_names_.end(), request->names[i]); + if (it != command_axis_names_.end()) + { + auto dist = std::distance(command_axis_names_.begin(), it); + std::size_t cmd_itf_index = + dist >= 0 ? static_cast(dist) + : throw std::runtime_error("Joint not found in command joint names"); + double pos = (request->positions.size() != 0) ? request->positions[i] + : std::numeric_limits::quiet_NaN(); + + if (request->positions.size() != 0) + { + RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'", request->names[i].c_str()); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), "Resetting dof '%s' position to %f", + request->names[i].c_str(), pos); + } + double vel = (request->velocities.size() != 0) ? request->velocities[i] + : std::numeric_limits::quiet_NaN(); + double accel = (request->accelerations.size() != 0) + ? request->accelerations[i] + : std::numeric_limits::quiet_NaN(); + reset_flags_local[cmd_itf_index] = {true, pos, vel, accel}; + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", + request->names[i].c_str()); + response->ok = false; + } + } + + reset_dofs_flags_.writeFromNonRT(reset_flags_local); + }; + + reset_dofs_service_ = get_node()->create_service( + "~/reset_dofs", reset_dofs_service_callback); + + return CallbackReturn::SUCCESS; +} + +void MultiTimeTrajectoryController::reference_callback( + const std::shared_ptr msg, const bool reliable) +{ + if (reliable) + { + last_reliable_reference_ = *msg; + is_reliable_update_pending_ = true; + add_new_trajectory_msg(msg, true); + } + else + { + // drop the message if we have a reliable update pending + if (!is_reliable_update_pending_) + { + last_reference_ = *msg; + add_new_trajectory_msg(msg); + } + } +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_activate( + const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_node()->get_logger(), "MAC activating..."); + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + + // order all axes in the storage + for (const auto & interface : params_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto dist = std::distance(allowed_interface_types_.begin(), it); + std::size_t index = + dist >= 0 ? static_cast(dist) + : throw std::runtime_error("Interface not found in allowed interface types"); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_axis_names_, interface, axis_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), axis_command_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + for (const auto & interface : params_.state_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto dist = std::distance(allowed_interface_types_.begin(), it); + std::size_t index = + dist >= 0 ? static_cast(dist) + : throw std::runtime_error("Interface not found in allowed interface types"); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, params_.axes, interface, axis_state_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + interface.c_str(), axis_state_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + + traj_external_point_ptr_ = std::make_shared(); + traj_msg_external_point_ptr_.writeFromNonRT( + std::shared_ptr()); + + subscriber_is_active_ = true; + + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + + initialize_current_state(); + + RCLCPP_DEBUG(get_node()->get_logger(), "MAC successfully activated"); + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + rt_has_pending_goal_.writeFromNonRT(false); + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled during deactivate transition."); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + + for (size_t index = 0; index < dof_; ++index) + { + if (has_position_command_interface_) + { + if (!axis_command_interface_[0][index].get().set_value( + axis_command_interface_[0][index].get().get_value())) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), "Failed to set position command interface to current value for " + << command_axis_names_[index]); + } + } + + if (has_velocity_command_interface_) + { + if (!axis_command_interface_[1][index].get().set_value(0.0)) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set velocity command interface to 0 for " << command_axis_names_[index]); + } + } + + if (has_acceleration_command_interface_) + { + if (!axis_command_interface_[2][index].get().set_value(0.0)) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set acceleration command interface to 0 for " << command_axis_names_[index]); + } + } + + // TODO(anyone): How to halt when using effort commands? + if (has_effort_command_interface_) + { + if (!axis_command_interface_[3][index].get().set_value(0.0)) + { + RCLCPP_WARN_STREAM( + get_node()->get_logger(), + "Failed to set effort command interface to 0 for " << command_axis_names_[index]); + } + } + } + + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + axis_command_interface_[index].clear(); + axis_state_interface_[index].clear(); + } + release_interfaces(); + + subscriber_is_active_ = false; + + traj_external_point_ptr_.reset(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_error( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +bool MultiTimeTrajectoryController::reset() +{ + subscriber_is_active_ = false; + axis_command_subscriber_.reset(); + + for (const auto & pid : pids_) + { + if (pid) + { + pid->reset(); + } + } + + traj_external_point_ptr_.reset(); + + return true; +} + +controller_interface::CallbackReturn MultiTimeTrajectoryController::on_shutdown( + const rclcpp_lifecycle::State &) +{ + // TODO(karsten1987): what to do? + + return CallbackReturn::SUCCESS; +} + +void MultiTimeTrajectoryController::publish_state( + const rclcpp::Time & time, const bool first_sample, const std::vector & segment_start) +{ + // TODO(henrygerardmoore): add any useful debug/state info to this publish + // active state? goals and tolerances, internal state, etc. + if (state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.reference = last_reference_; + state_publisher_->msg_.reference_reliable = last_reliable_reference_; + state_publisher_->msg_.feedback = state_current_; + state_publisher_->msg_.error = state_error_; + state_publisher_->msg_.using_odometry_feedback = params_.use_feedback; + state_publisher_->msg_.trajectory_active = has_active_trajectory(); + state_publisher_->msg_.last_odom_feedback = last_odom_feedback_; + state_publisher_->msg_.goal_recvd = trajectory_msg_recvd_; + state_publisher_->msg_.goal = *traj_external_point_ptr_->get_trajectory_msg(); + state_publisher_->msg_.first_sample_in_trajectory = first_sample; + state_publisher_->msg_.reset_dofs_position = reset_dofs_positions_; + state_publisher_->msg_.trajectory_start_time = traj_external_point_ptr_->start_time(); + auto const times_before_traj = traj_external_point_ptr_->time_before_trajectory(); + state_publisher_->msg_.time_before_trajectory.clear(); + for (auto const & time_before_traj : times_before_traj) + { + state_publisher_->msg_.time_before_trajectory.push_back(time_before_traj); + } + state_publisher_->msg_.interpoland_times_ns = traj_external_point_ptr_->interpoland_time_ns(); + state_publisher_->msg_.interpoint_times_ns = traj_external_point_ptr_->inter_point_time_ns(); + state_publisher_->msg_.interpolation_state_a = + traj_external_point_ptr_->interpolation_state_a(); + state_publisher_->msg_.interpolation_state_b = + traj_external_point_ptr_->interpolation_state_b(); + state_publisher_->msg_.state_before_trajectory = + traj_external_point_ptr_->state_before_trajectory(); + state_publisher_->msg_.state_after_interpolation = + traj_external_point_ptr_->state_after_interp(); + state_publisher_->msg_.state_after_joint_limit = + traj_external_point_ptr_->state_after_joint_limit(); + state_publisher_->msg_.state_previous = traj_external_point_ptr_->previous_state(); + state_publisher_->msg_.segment_start = segment_start; + + read_commands_from_command_interfaces(command_current_); + state_publisher_->msg_.output = command_current_; + + state_publisher_->unlockAndPublish(); + } + + if (splines_output_publisher_ && splines_output_publisher_->trylock()) + { + splines_output_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; + splines_output_publisher_->msg_.state_after_interpolation = splines_state_; + + splines_output_publisher_->unlockAndPublish(); + } +} + +void MultiTimeTrajectoryController::topic_callback( + const std::shared_ptr msg) +{ + if (!validate_trajectory_msg(*msg)) + { + return; + } + // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement + // always replace old msg with new one for now + if (subscriber_is_active_) + { + add_new_trajectory_msg(msg); + rt_is_holding_.writeFromNonRT(false); + } +}; + +rclcpp_action::GoalResponse MultiTimeTrajectoryController::goal_received_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); + + // Precondition: Running controller + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!validate_trajectory_msg(goal->trajectory)) + { + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse MultiTimeTrajectoryController::goal_cancelled_callback( + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + RCLCPP_INFO( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + rt_has_pending_goal_.writeFromNonRT(false); + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + + // Enter hold current position mode + add_new_trajectory_msg(set_hold_position()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MultiTimeTrajectoryController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + // mark a pending goal + rt_has_pending_goal_.writeFromNonRT(true); + + // Update new trajectory + { + preempt_active_goal(); + auto traj_msg = + std::make_shared(goal_handle->get_goal()->trajectory); + + add_new_trajectory_msg(traj_msg); + rt_is_holding_.writeFromNonRT(false); + } + + // Update the active goal + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->preallocated_feedback_->axis_names = params_.axes; + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} + +void MultiTimeTrajectoryController::compute_error( + std::vector & error, const std::vector & current, + const std::vector & desired) const +{ + error.resize(current.size()); + for (std::size_t index = 0; index < current.size(); ++index) + { + // error defined as the difference between current and desired + if (axis_angle_wraparound_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pi traj) +{ + bool position_empty = std::any_of( + traj.begin(), traj.end(), + [](control_msgs::msg::AxisTrajectoryPoint point) { return std::isnan(point.position); }); + bool velocity_empty = std::any_of( + traj.begin(), traj.end(), + [](control_msgs::msg::AxisTrajectoryPoint point) { return std::isnan(point.velocity); }); + bool acceleration_empty = std::any_of( + traj.begin(), traj.end(), + [](control_msgs::msg::AxisTrajectoryPoint point) { return std::isnan(point.acceleration); }); + return position_empty && velocity_empty && acceleration_empty; +} + +void MultiTimeTrajectoryController::sort_to_local_axis_order( + std::shared_ptr trajectory_msg) const +{ + // rearrange all points in the trajectory message based on mapping + std::vector mapping_vector = mapping(trajectory_msg->axis_names, params_.axes); + + // this will create an empty AxisTrajectory for any unfilled axes, but that is handled in + // `Trajectory::update` + std::vector to_return(dof_); + std::size_t message_index = 0; + for (auto const internal_index : mapping_vector) + { + if (!isEmpty(trajectory_msg->axis_trajectories[message_index].axis_points)) + { + to_return[internal_index] = trajectory_msg->axis_trajectories[message_index]; + } + ++message_index; + } + trajectory_msg->axis_trajectories = to_return; +} + +bool MultiTimeTrajectoryController::validate_trajectory_msg( + const control_msgs::msg::MultiAxisTrajectory & trajectories) const +{ + const auto trajectory_start_time = static_cast(trajectories.header.stamp); + + if (trajectories.axis_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); + return false; + } + + if (trajectories.axis_trajectories.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; + } + + for (std::size_t axis_index = 0; axis_index < trajectories.axis_names.size(); ++axis_index) + { + auto const & trajectory = trajectories.axis_trajectories[axis_index]; + const std::string & incoming_axis_name = trajectories.axis_names[axis_index]; + // If partial axes goals are not allowed, goal should specify all controller axes + + // 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 const trajectory_end_time = + trajectory_start_time + trajectory.axis_points.back().time_from_start; + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory for axis %zu with non-zero start time (%f) that ends in the past " + "(%f)", + axis_index, trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + + auto it = std::find(params_.axes.begin(), params_.axes.end(), incoming_axis_name); + if (it == params_.axes.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Incoming axis %s doesn't match the controller's axes.", + incoming_axis_name.c_str()); + return false; + } + + if (!params_.allow_nonzero_velocity_at_trajectory_end) + { + auto const final_vel = trajectory.axis_points.back().velocity; + if (fabs(final_vel) > std::numeric_limits::epsilon()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Velocity of last trajectory point of axis %s is not zero: %.15f", + incoming_axis_name.c_str(), final_vel); + return false; + } + } + + rclcpp::Duration previous_traj_time(0ms); + for (size_t i = 0; i < trajectory.axis_points.size(); ++i) + { + if ( + (i > 0) && + (rclcpp::Duration(trajectory.axis_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.axis_points[i].time_from_start).seconds()); + return false; + } + previous_traj_time = trajectory.axis_points[i].time_from_start; + + const auto & points = trajectory.axis_points; + bool traj_has_position = !std::any_of( + points.begin(), points.end(), + [](control_msgs::msg::AxisTrajectoryPoint point) { return std::isnan(point.position); }); + bool traj_has_velocity = !std::any_of( + points.begin(), points.end(), + [](control_msgs::msg::AxisTrajectoryPoint point) { return std::isnan(point.velocity); }); + bool traj_has_acceleration = !std::any_of( + points.begin(), points.end(), [](control_msgs::msg::AxisTrajectoryPoint point) + { return std::isnan(point.acceleration); }); + + if (!(traj_has_position || traj_has_velocity || traj_has_acceleration)) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Empty trajectory for axis %s", incoming_axis_name.c_str()); + return false; + } + + if (!params_.allow_integration_in_goal_trajectories && !traj_has_position) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "No position in trajectory for axis %s and integration of goal trajectories is disabled", + incoming_axis_name.c_str()); + return false; + } + } + } + return true; +} + +void MultiTimeTrajectoryController::add_new_trajectory_msg( + const std::shared_ptr & traj_msg, const bool reliable) +{ + if (reliable) + { + std::lock_guard guard(mutex_); + + // merge reliable message if previous message not yet processed + + // Check if a new external message has already been received before from nonRT threads + auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); + auto new_external_msg = traj_msg_external_point_ptr_.readFromNonRT(); + + // previous reliable message not yet processed + if ((current_external_msg != *new_external_msg) && (*new_external_msg)) + { + // replace external message with new message for the same axes + for (std::size_t i = 0; i < (*new_external_msg)->axis_names.size(); ++i) + { + auto iter = std::find( + traj_msg->axis_names.begin(), traj_msg->axis_names.end(), + (*new_external_msg)->axis_names[i]); + if (iter == traj_msg->axis_names.end()) continue; + + auto index = std::distance(traj_msg->axis_names.begin(), iter); + + // if new message has only nan values, skip + if (std::all_of( + traj_msg->axis_trajectories[static_cast(index)].axis_points.cbegin(), + traj_msg->axis_trajectories[static_cast(index)].axis_points.cend(), + [](const control_msgs::msg::AxisTrajectoryPoint & point) + { + return std::isnan(point.position) && std::isnan(point.velocity) && + std::isnan(point.acceleration); + })) + { + continue; + } + + (*new_external_msg)->axis_trajectories[i].axis_points = + traj_msg->axis_trajectories[static_cast(index)].axis_points; + } + + // merge axes from new message to external message, that are not in external message + for (std::size_t i = 0; i < traj_msg->axis_names.size(); ++i) + { + // if new message has only nan values, skip + if (std::all_of( + traj_msg->axis_trajectories[i].axis_points.cbegin(), + traj_msg->axis_trajectories[i].axis_points.cend(), + [](const control_msgs::msg::AxisTrajectoryPoint & point) + { + return std::isnan(point.position) && std::isnan(point.velocity) && + std::isnan(point.acceleration); + })) + { + continue; + } + + auto iter = std::find( + (*new_external_msg)->axis_names.begin(), (*new_external_msg)->axis_names.end(), + traj_msg->axis_names[i]); + if (iter == (*new_external_msg)->axis_names.end()) + { + (*new_external_msg)->axis_names.push_back(traj_msg->axis_names[i]); + (*new_external_msg)->axis_trajectories.push_back(traj_msg->axis_trajectories[i]); + } + } + } + else + { + traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + } + } + else + { + traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + } +} + +void MultiTimeTrajectoryController::preempt_active_goal() +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled due to new incoming action."); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } +} + +std::shared_ptr +MultiTimeTrajectoryController::set_hold_position() +{ + // Command to stay at current position + for (std::size_t i = 0; i < dof_; ++i) + { + hold_position_msg_ptr_->axis_trajectories[i].axis_points = {state_current_[i]}; + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].velocity = 0; + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].acceleration = 0; + } + + // set flag, otherwise tolerances will be checked with holding position too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + +std::shared_ptr +MultiTimeTrajectoryController::set_success_trajectory_point() +{ + // set last command to be repeated at success, no matter if it has nonzero velocity or + // acceleration + + for (std::size_t i = 0; i < dof_; ++i) + { + hold_position_msg_ptr_->axis_trajectories[i].axis_points = { + traj_external_point_ptr_->get_trajectory_msg()->axis_trajectories[i].axis_points.back()}; + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].time_from_start = + rclcpp::Duration(0, 0); + } + + // set flag, otherwise tolerances will be checked with success_trajectory_point too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + +bool MultiTimeTrajectoryController::contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type) +{ + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); +} + +bool MultiTimeTrajectoryController::has_active_trajectory() const +{ + return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); +} + +void MultiTimeTrajectoryController::update_pids() +{ + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.axes_map.at(params_.axes[i]); + if (pids_[i]) + { + // update PIDs with gains from ROS parameters + pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + else + { + // Init PIDs with gains from ROS parameters + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } +} + +bool MultiTimeTrajectoryController::initialize_current_state() +{ + // Initialize current state storage if hardware state has tracking offset + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) + std::vector state; + state.resize(dof_); + + // Handle restart of controller by reading from commands if + // those are not nan + if (!read_state_from_command_interfaces(state)) + { + // Initialize current state storage from hardware + if (!read_state_from_hardware(state)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to read feedback state from hardware"); + return false; + }; + } + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + last_commanded_time_ = std::vector{dof_, rclcpp::Time()}; + + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); + rt_is_holding_.writeFromNonRT(true); + + current_state_initialized_ = true; + + return true; +} + +void MultiTimeTrajectoryController::init_hold_position_msg() +{ + hold_position_msg_ptr_ = std::make_shared(); + hold_position_msg_ptr_->header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + hold_position_msg_ptr_->axis_names = params_.axes; + hold_position_msg_ptr_->axis_trajectories.resize(dof_); + for (std::size_t i = 0; i < dof_; ++i) + { + hold_position_msg_ptr_->axis_trajectories[i].axis_points.resize( + 1, emptyTrajectoryPoint()); // a trivial msg only + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].velocity = + std::numeric_limits::quiet_NaN(); + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].acceleration = + std::numeric_limits::quiet_NaN(); + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].effort = + std::numeric_limits::quiet_NaN(); + } + if (has_velocity_command_interface_ || has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns velocity points in any case + for (std::size_t i = 0; i < dof_; ++i) + { + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].velocity = 0; + } + } + if (has_acceleration_command_interface_) + { + // add acceleration, so that trajectory sampling returns acceleration points in any case + for (std::size_t i = 0; i < dof_; ++i) + { + hold_position_msg_ptr_->axis_trajectories[i].axis_points[0].acceleration = 0; + } + } +} + +} // namespace multi_time_trajectory_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + multi_time_trajectory_controller::MultiTimeTrajectoryController, + controller_interface::ControllerInterface) diff --git a/multi_time_trajectory_controller/src/multi_time_trajectory_controller_parameters.yaml b/multi_time_trajectory_controller/src/multi_time_trajectory_controller_parameters.yaml new file mode 100644 index 0000000000..83173be82e --- /dev/null +++ b/multi_time_trajectory_controller/src/multi_time_trajectory_controller_parameters.yaml @@ -0,0 +1,175 @@ +multi_time_trajectory_controller: + axes: { + type: string_array, + default_value: [], + description: "Axis names to control and listen to", + read_only: true, + validation: { + unique<>: null, + } + } + axes_is_angular: { + type: bool_array, + default_value: [], + description: "Axis is angular or linear. Should be same size as axes.", + read_only: true, + } + command_axes: { + type: string_array, + default_value: [], + description: "Axis names to control. This parameters is used if MAC is used in a controller chain where command and state interfaces don't have same names.", + read_only: true, + validation: { + unique<>: null, + } + } + command_interfaces: { + type: string_array, + default_value: [], + description: "Command interfaces provided by the hardware interface for all axes", + read_only: true, + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration", "effort",]], + "multi_time_trajectory_controller::command_interface_type_combinations": null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "State interfaces provided by the hardware for all axes.", + read_only: true, + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration",]], + "multi_time_trajectory_controller::state_interface_type_combinations": null, + } + } + hold_last_velocity: { + type: bool, + default_value: true, + read_only: true, + } + use_feedback: { + type: bool, + default_value: true, + read_only: true, + } + open_loop_control: { + type: bool, + default_value: false, + description: "Use controller in open-loop control mode + \n\n + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n + * It deactivates the feedback control, see the ``gains`` structure. + \n\n + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + \n\n + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started.\n", + read_only: true, + } + allow_integration_in_goal_trajectories: { + type: bool, + default_value: false, + description: "Allow integration in goal trajectories to accept goals without position or velocity specified", + } + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowAxisTrajectory)", + read_only: true, + validation: { + gt_eq: [0.1] + } + } + interpolation_method: { + type: string, + default_value: "splines", + description: "The type of interpolation to use, if any", + read_only: true, + validation: { + one_of<>: [["splines", "none"]], + } + } + allow_nonzero_velocity_at_trajectory_end: { + type: bool, + default_value: false, + description: "If false, the last velocity point has to be zero or the goal will be rejected", + } + cmd_timeout: { + type: double, + default_value: 0.0, # seconds + description: "Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. + If zero, timeout is deactivated", + } + gains: + __map_axes: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain :math:`k_p` for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain :math:`k_i` for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain :math:`k_d` for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integral clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling :math:`k_{ff}` of velocity" + } + angle_wraparound: { + type: bool, + default_value: false, + description: "[deprecated] For axes that wrap around (ie. are continuous). + Normalizes position-error to -pi to pi." + } + constraints: + stopped_velocity_tolerance: { + type: double, + default_value: 0.01, + description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.", + } + goal_time: { + type: double, + default_value: 0.0, + description: "Time tolerance for achieving trajectory goal before or after commanded time. + If set to zero, the controller will wait a potentially infinite amount of time.", + validation: { + gt_eq: [0.0], + } + } + __map_axes: + trajectory: { + type: double, + default_value: 0.0, + description: "Per-axis trajectory offset tolerance during movement.", + } + goal: { + type: double, + default_value: 0.0, + description: "Per-axis trajectory offset tolerance at the goal position.", + } + + joint_limiter_type: { + type: string, + default_value: "", + description: "The type of joint limiter to use. If empty, no joint limiter will be used. + Per default, the simple joint limiter implementation from 'joint_limits' package is used. + For other official implementations see 'joint_limiters' package in 'ros2_control' repository.", + } diff --git a/multi_time_trajectory_controller/src/trajectory.cpp b/multi_time_trajectory_controller/src/trajectory.cpp new file mode 100644 index 0000000000..249eb7c5d3 --- /dev/null +++ b/multi_time_trajectory_controller/src/trajectory.cpp @@ -0,0 +1,751 @@ +// 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 "multi_time_trajectory_controller/trajectory.hpp" +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "control_msgs/msg/axis_trajectory.hpp" +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "control_msgs/msg/multi_axis_trajectory.hpp" +#include "hardware_interface/macros.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace multi_time_trajectory_controller +{ +namespace +{ +// Safe default joint kinematic limits for Ruckig, in case none is defined in the URDF or +// joint_limits.yaml +constexpr double DEFAULT_MAX_VELOCITY = 1.5; // rad/s +constexpr double DEFAULT_MAX_ACCELERATION = 5.0; // rad/s^2 +constexpr double DEFAULT_MAX_JERK = 200.0; // rad/s^3 + +// convert JointTrajectoryPoint to vector of AxisTrajectoryPoint +bool convert_to_axis_trajectory_points_for_joint_limiting( + const trajectory_msgs::msg::JointTrajectoryPoint & joint_trajectory_point, + std::vector & axis_trajectory_points) +{ + if ( + (axis_trajectory_points.size() != joint_trajectory_point.positions.size()) || + (axis_trajectory_points.size() != joint_trajectory_point.velocities.size())) + { + return false; + } + + for (size_t i = 0; i < axis_trajectory_points.size(); ++i) + { + axis_trajectory_points[i].position = joint_trajectory_point.positions[i]; + axis_trajectory_points[i].velocity = joint_trajectory_point.velocities[i]; + if (joint_trajectory_point.accelerations.size() > i) + { + axis_trajectory_points[i].acceleration = joint_trajectory_point.accelerations[i]; + } + } + + return true; +} + +// convert vector of AxisTrajectoryPoint to JointTrajectoryPoint +void convert_to_joint_trajectory_point_for_joint_limiting( + const std::vector & axis_trajectory_points, + trajectory_msgs::msg::JointTrajectoryPoint & joint_trajectory_point) +{ + joint_trajectory_point.positions.clear(); + joint_trajectory_point.velocities.clear(); + joint_trajectory_point.accelerations.clear(); + + for (size_t i = 0; i < axis_trajectory_points.size(); ++i) + { + if (!std::isnan(axis_trajectory_points[i].position)) + { + joint_trajectory_point.positions.push_back(axis_trajectory_points[i].position); + } + + if (!std::isnan(axis_trajectory_points[i].velocity)) + { + joint_trajectory_point.velocities.push_back(axis_trajectory_points[i].velocity); + } + + if (!std::isnan(axis_trajectory_points[i].acceleration)) + { + joint_trajectory_point.accelerations.push_back(axis_trajectory_points[i].acceleration); + } + } +} +} // namespace + +Trajectory::Trajectory() +: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false) +{ +} + +Trajectory::Trajectory(std::shared_ptr joint_trajectory) +: trajectory_msg_(joint_trajectory), + trajectory_start_time_(static_cast(joint_trajectory->header.stamp)), + sampled_already_(false) +{ +} + +Trajectory::Trajectory( + const rclcpp::Time & current_time, + const std::vector & current_point, + std::shared_ptr joint_trajectory) +: trajectory_msg_(joint_trajectory), + trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) +{ + set_point_before_trajectory_msg( + std::vector{current_point.size(), current_time}, current_point); + // update(joint_trajectory); +} + +void Trajectory::set_point_before_trajectory_msg( + const std::vector & current_time, + const std::vector & current_point, + const std::vector & joints_angle_wraparound) +{ + time_before_traj_msg_ = current_time; + state_before_traj_msg_ = current_point; + std::size_t const num_axes = state_before_traj_msg_.size(); + + // Compute offsets due to wrapping joints + wraparound_joint( + state_before_traj_msg_, trajectory_msg_->axis_trajectories, joints_angle_wraparound); + + if (trajectory_msg_) + { + for (std::size_t axis_index = 0; axis_index < num_axes; ++axis_index) + { + auto iter = trajectory_msg_->axis_trajectories[axis_index].axis_points.begin(); + auto const traj_start_time = + rclcpp::Time(trajectory_msg_->header.stamp, current_time[axis_index].get_clock_type()); + auto const end = trajectory_msg_->axis_trajectories[axis_index].axis_points.end(); + while (iter != end && + iter->time_from_start + traj_start_time < time_before_traj_msg_[axis_index]) + { + ++iter; + } + if (iter == trajectory_msg_->axis_trajectories[axis_index].axis_points.begin()) + { + // we don't need to get rid of any points, so skip the copy operation (O(n)) + continue; + } + if (iter == end) + { + trajectory_msg_->axis_trajectories[axis_index].axis_points = { + state_before_traj_msg_[axis_index]}; + + // set the new trajectory to just be the single reset point + trajectory_msg_->axis_trajectories[axis_index].axis_points.back().time_from_start = + time_before_traj_msg_[axis_index] - traj_start_time; + // we replaced the whole trajectory, no need to copy below + continue; + } + + std::vector axis_traj{ + iter, trajectory_msg_->axis_trajectories[axis_index].axis_points.end()}; + trajectory_msg_->axis_trajectories[axis_index].axis_points = axis_traj; + } + } +} + +void wraparound_joint( + const std::vector & current_position, + std::vector & trajectory, + const std::vector & joints_angle_wraparound) +{ + double dist; + + // joints_angle_wraparound is even empty, or has the same size as the number of joints + for (size_t i = 0; i < joints_angle_wraparound.size(); i++) + { + if (!joints_angle_wraparound[i]) + { + continue; + } + + auto curr_pos = current_position[i].position; + for (size_t j = 0; j < trajectory[i].axis_points.size(); j++) + { + // skip if current or next position is NaN + if (std::isnan(curr_pos) || std::isnan(trajectory[i].axis_points[j].position)) + { + continue; + } + + dist = angles::shortest_angular_distance(trajectory[i].axis_points[0].position, curr_pos); + + // Deal with singularity at M_PI shortest distance + if (std::abs(std::abs(dist) - M_PI) < 1e-9) + { + dist = curr_pos > trajectory[i].axis_points[j].position ? std::abs(dist) : -std::abs(dist); + } + + auto old_traj_pos = trajectory[i].axis_points[j].position; + trajectory[i].axis_points[j].position = curr_pos - dist; + RCLCPP_DEBUG( + rclcpp::get_logger("trajectory"), "Wraparound Axis %zu: TrajPos[%zu]: %f -> %f, Prev: %f", + i, j, old_traj_pos, trajectory[i].axis_points[0].position, curr_pos); + + // set this trajectory point as the new current position for next trajectory point + curr_pos = trajectory[i].axis_points[j].position; + } + } +} + +void Trajectory::update( + std::shared_ptr multi_axis_trajectory, + const std::vector & joint_limits, const rclcpp::Duration & period, + rclcpp::Time const & time) +{ + // start by setting the start time of the trajectory if we're supposed to start it right away + if (rclcpp::Time(multi_axis_trajectory->header.stamp).seconds() == 0.0) + { + multi_axis_trajectory->header.stamp = time; + trajectory_start_time_ = time; + } + else + { + trajectory_start_time_ = + rclcpp::Time(multi_axis_trajectory->header.stamp, time.get_clock_type()); + } + + // if we don't already have a trajectory message, skip the below block + if (trajectory_msg_) + { + // ensure previous and current trajectories are same size + if ( + multi_axis_trajectory->axis_trajectories.size() != trajectory_msg_->axis_trajectories.size()) + { + throw std::runtime_error(fmt::format( + "Previous and newly received trajectory are of different sizes, {} and {}", + trajectory_msg_->axis_trajectories.size(), + multi_axis_trajectory->axis_trajectories.size())); + } + + // first, see if we need to do anything other than just replace the whole trajectory_msg_ + bool const replace_subset = std::any_of( + multi_axis_trajectory->axis_trajectories.begin(), + multi_axis_trajectory->axis_trajectories.end(), + [](control_msgs::msg::AxisTrajectory traj) { return traj.axis_points.empty(); }); + + if (replace_subset) + { + // logic to replace only certain axes with the updated trajectory + // first, calculate the time offset to apply to the old axis trajectories (since we have one + // time_from_start) + auto const time_offset = static_cast(multi_axis_trajectory->header.stamp) - + static_cast(trajectory_msg_->header.stamp); + for (std::size_t i = 0; i < multi_axis_trajectory->axis_trajectories.size(); ++i) + { + auto & traj = multi_axis_trajectory->axis_trajectories[i].axis_points; + if (traj.empty()) + { + // copy the old one into the new message (which will replace ours) + traj = trajectory_msg_->axis_trajectories[i].axis_points; + for (auto & point : traj) + { + // update time of the old trajectory + point.time_from_start = rclcpp::Duration(point.time_from_start) - time_offset; + } + } + } + } + } + + // replace the old message with the new one + trajectory_msg_ = multi_axis_trajectory; + + sampled_already_ = false; + + size_t dim = multi_axis_trajectory->axis_names.size(); + + if (previous_state_.size() != dim) + { + previous_state_.resize(dim); + for (std::size_t i = 0; i < dim; ++i) + { + previous_state_[i].position = std::numeric_limits::quiet_NaN(); + previous_state_[i].velocity = std::numeric_limits::quiet_NaN(); + previous_state_[i].acceleration = std::numeric_limits::quiet_NaN(); + } + } + output_state_after_interp_ = previous_state_; + output_state_after_joint_limit_ = previous_state_; + inter_point_time_ns_.resize(dim); + interpoland_time_ns_.resize(dim); + interpolation_state_a_.resize(dim); + interpolation_state_b_.resize(dim); +} + +std::vector Trajectory::sample( + const rclcpp::Time & sample_time, + const joint_trajectory_controller::interpolation_methods::InterpolationMethod + interpolation_method, + std::vector & output_state, + std::vector & start_segment_itr, + std::vector & end_segment_itr, const rclcpp::Duration & period, + std::unique_ptr> & + joint_limiter, + std::vector & splines_state, bool hold_last_velocity) +{ + THROW_ON_NULLPTR(trajectory_msg_) + std::size_t num_axes = trajectory_msg_->axis_trajectories.size(); + std::vector is_valid(num_axes, false); + + if ( + trajectory_msg_->axis_trajectories.empty() || + std::any_of( + trajectory_msg_->axis_trajectories.begin(), trajectory_msg_->axis_trajectories.end(), + [](control_msgs::msg::AxisTrajectory traj) { return traj.axis_points.empty(); })) + { + // empty axis trajectories or empty axis points (invalid) + start_segment_itr.resize(trajectory_msg_->axis_trajectories.size()); + end_segment_itr.resize(trajectory_msg_->axis_trajectories.size()); + for (std::size_t i = 0; i < trajectory_msg_->axis_trajectories.size(); ++i) + { + start_segment_itr[i] = end(i); + end_segment_itr[i] = end(i); + } + return is_valid; + } + + // first sampling of this trajectory + if (!sampled_already_) + { + if (trajectory_start_time_.seconds() == 0.0) + { + trajectory_start_time_ = sample_time; + // set this so we can know when the trajectory started later + trajectory_msg_->header.stamp = sample_time; + } + + sampled_already_ = true; + } + + // sampling before the current point + if (std::any_of( + time_before_traj_msg_.begin(), time_before_traj_msg_.end(), + [&](rclcpp::Time time) { return sample_time < time; })) + { + return is_valid; + } + + // output_state splines_state + if (output_state.size() < num_axes) + { + output_state.resize(num_axes); + } + if (splines_state.size() < num_axes) + { + splines_state.resize(num_axes); + } + + bool enforce_joint_limits = false; + previous_state_cached_ = previous_state_; + for (std::size_t axis_index = 0; axis_index < num_axes; ++axis_index) + { + auto & trajectory = trajectory_msg_->axis_trajectories[axis_index]; + auto & first_point_in_msg = trajectory.axis_points[0]; + const rclcpp::Time first_point_timestamp = + trajectory_start_time_ + first_point_in_msg.time_from_start; + + // current time hasn't reached traj time of the first point in the msg yet + if (sample_time < first_point_timestamp) + { + // If interpolation is disabled, just forward the next waypoint + if ( + interpolation_method == + joint_trajectory_controller::interpolation_methods::InterpolationMethod::NONE) + { + output_state = state_before_traj_msg_; + previous_state_ = state_before_traj_msg_; + output_state_after_interp_ = state_before_traj_msg_; + output_state_after_joint_limit_ = state_before_traj_msg_; + } + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + state_before_traj_msg_[axis_index], first_point_in_msg, + (first_point_timestamp - time_before_traj_msg_[axis_index]).seconds()); + + interpolation_state_a_[axis_index] = state_before_traj_msg_[axis_index]; + interpolation_state_b_[axis_index] = first_point_in_msg; + inter_point_time_ns_[axis_index] = + (first_point_timestamp - time_before_traj_msg_[axis_index]).nanoseconds(); + interpoland_time_ns_[axis_index] = + (sample_time - time_before_traj_msg_[axis_index]).nanoseconds(); + + interpolate_between_points( + time_before_traj_msg_[axis_index], state_before_traj_msg_[axis_index], + first_point_timestamp, first_point_in_msg, sample_time, false, output_state[axis_index], + period, splines_state[axis_index], axis_index); + + output_state_after_interp_[axis_index] = output_state[axis_index]; + } + start_segment_itr[axis_index] = begin(axis_index); // no segments before the first + end_segment_itr[axis_index] = begin(axis_index); + + enforce_joint_limits = true; + is_valid[axis_index] = true; + continue; + } + + // time_from_start + trajectory time is the expected arrival time of trajectory + const auto point_before_last_idx = trajectory.axis_points.size() - 1; + + // if the segment is within the trajectory, we will continue after finding it, as the rest of + // the loop is for if we've finished the trajectory + bool should_continue = false; + for (size_t i = 0; i < point_before_last_idx; ++i) + { + auto & point = trajectory.axis_points[i]; + auto & next_point = trajectory.axis_points[i + 1]; + + const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start; + const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start; + + if (sample_time >= t0 && sample_time < t1) + { + // If interpolation is disabled, just forward the next waypoint + if ( + interpolation_method == + joint_trajectory_controller::interpolation_methods::InterpolationMethod::NONE) + { + output_state[axis_index] = next_point; + } + // Do interpolation + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives(point, next_point, (t1 - t0).seconds()); + + inter_point_time_ns_[axis_index] = (t1 - t0).nanoseconds(); + interpoland_time_ns_[axis_index] = (sample_time - t0).nanoseconds(); + interpolation_state_a_[axis_index] = point; + interpolation_state_b_[axis_index] = next_point; + + if (!interpolate_between_points( + t0, point, t1, next_point, sample_time, false, output_state[axis_index], period, + splines_state[axis_index], axis_index)) + { + is_valid[axis_index] = false; + should_continue = true; + break; + } + } + start_segment_itr[axis_index] = begin(axis_index) + static_cast(i); + end_segment_itr[axis_index] = begin(axis_index) + static_cast(i + 1); + + enforce_joint_limits = true; + is_valid[axis_index] = true; + should_continue = true; + break; + } + } + if (should_continue) + { + output_state_after_interp_[axis_index] = output_state[axis_index]; + continue; + } + + // this trajectory is valid, set the start and end segment iterators + start_segment_itr[axis_index] = --end(axis_index); + end_segment_itr[axis_index] = end(axis_index); + is_valid[axis_index] = true; + + // perform the existing JTC functionality if we don't want to hold the last velocity + if (!hold_last_velocity) + { + output_state[axis_index] = (*start_segment_itr[axis_index]); + output_state_after_interp_[axis_index] = output_state[axis_index]; + if (std::isnan(output_state[axis_index].velocity)) + { + output_state[axis_index].velocity = 0.0; + } + if (std::isnan(output_state[axis_index].acceleration)) + { + output_state[axis_index].acceleration = 0.0; + } + continue; + } + + // hold the last velocity, so we extrapolate from the last point's vel and accel + auto & last_point = trajectory.axis_points.back(); + rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; + + if (last_point.acceleration != 0 && !std::isnan(last_point.acceleration)) + { + last_point.velocity += last_point.acceleration * period.seconds(); + // remember velocity over multiple calls + } + if (last_point.velocity != 0 && !std::isnan(last_point.velocity)) + { + last_point.position += last_point.velocity * period.seconds(); + // remember velocity over multiple calls + } + + // do not do splines when trajectory has finished because the time is achieved + inter_point_time_ns_[axis_index] = (t0 - t0).nanoseconds(); + interpoland_time_ns_[axis_index] = (sample_time - t0).nanoseconds(); + interpolation_state_a_[axis_index] = last_point; + interpolation_state_b_[axis_index] = last_point; + if (!interpolate_between_points( + t0, last_point, t0, last_point, sample_time, true, output_state[axis_index], period, + splines_state[axis_index], axis_index)) + { + is_valid[axis_index] = false; + } + else + { + enforce_joint_limits = true; + } + + output_state_after_interp_[axis_index] = output_state[axis_index]; + } + + if (enforce_joint_limits && joint_limiter) + { + trajectory_msgs::msg::JointTrajectoryPoint prev_state_joint_traj_pt, output_state_joint_traj_pt; + convert_to_joint_trajectory_point_for_joint_limiting(previous_state_, prev_state_joint_traj_pt); + convert_to_joint_trajectory_point_for_joint_limiting(output_state, output_state_joint_traj_pt); + + joint_limiter->enforce(prev_state_joint_traj_pt, output_state_joint_traj_pt, period); + + convert_to_axis_trajectory_points_for_joint_limiting(output_state_joint_traj_pt, output_state); + } + output_state_after_joint_limit_ = output_state; + + previous_state_ = output_state; + return is_valid; +} + +bool Trajectory::interpolate_between_points( + const rclcpp::Time & time_a, const control_msgs::msg::AxisTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const control_msgs::msg::AxisTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, const bool skip_splines, + control_msgs::msg::AxisTrajectoryPoint & output, const rclcpp::Duration & period, + control_msgs::msg::AxisTrajectoryPoint & splines_state, std::size_t axis_index) +{ + // RCLCPP_WARN(rclcpp::get_logger("trajectory"), "New iteration"); + + rclcpp::Duration duration_so_far = sample_time - time_a; + rclcpp::Duration duration_btwn_points = time_b - time_a; + + output.position = std::numeric_limits::quiet_NaN(); + output.velocity = std::numeric_limits::quiet_NaN(); + output.acceleration = std::numeric_limits::quiet_NaN(); + output.effort = std::numeric_limits::quiet_NaN(); + output.time_from_start.sec = 0; + output.time_from_start.nanosec = 0; + + bool has_velocity = !std::isnan(state_a.velocity) && !std::isnan(state_b.velocity); + bool has_accel = !std::isnan(state_a.acceleration) && !std::isnan(state_b.acceleration); + + splines_state.position = std::numeric_limits::quiet_NaN(); + splines_state.velocity = std::numeric_limits::quiet_NaN(); + splines_state.acceleration = std::numeric_limits::quiet_NaN(); + splines_state.effort = std::numeric_limits::quiet_NaN(); + + if (!skip_splines) + { + auto generate_powers = [](int n, double x, double * powers) + { + powers[0] = 1.0; + for (int i = 1; i <= n; ++i) + { + powers[i] = powers[i - 1] * x; + } + }; + + if (duration_so_far.seconds() < 0.0) + { + duration_so_far = rclcpp::Duration::from_seconds(0.0); + has_velocity = has_accel = false; + RCLCPP_WARN(rclcpp::get_logger("trajectory"), "Duration so far is negative"); + } + if (duration_so_far.seconds() > duration_btwn_points.seconds()) + { + RCLCPP_WARN( + rclcpp::get_logger("trajectory"), + "Duration so far is smaller then duration between points"); + duration_so_far = duration_btwn_points; + has_velocity = has_accel = false; + } + + output.effort = state_b.position; + + double t[6]; + generate_powers(5, duration_so_far.seconds(), t); + + if (!has_velocity && !has_accel) + { + // do linear interpolation + double start_pos = state_a.position; + double end_pos = state_b.position; + + double coefficients[2] = {0.0, 0.0}; + coefficients[0] = start_pos; + if (duration_btwn_points.seconds() != 0.0) + { + coefficients[1] = (end_pos - start_pos) / duration_btwn_points.seconds(); + } + + output.position = t[0] * coefficients[0] + t[1] * coefficients[1]; + output.velocity = t[0] * coefficients[1]; + } + else if (has_velocity && !has_accel) + { + // do cubic interpolation + double T[4]; + generate_powers(3, duration_btwn_points.seconds(), T); + + double start_pos = state_a.position; + double start_vel = state_a.velocity; + double end_pos = state_b.position; + double end_vel = state_b.velocity; + + double coefficients[4] = {0.0, 0.0, 0.0, 0.0}; + coefficients[0] = start_pos; + coefficients[1] = start_vel; + if (duration_btwn_points.seconds() != 0.0) + { + coefficients[2] = + (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2]; + coefficients[3] = + (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3]; + } + + output.position = t[0] * coefficients[0] + t[1] * coefficients[1] + t[2] * coefficients[2] + + t[3] * coefficients[3]; + output.velocity = + t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + t[2] * 3.0 * coefficients[3]; + output.acceleration = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3]; + } + else if (has_velocity && has_accel) + { + // do quintic interpolation + double T[6]; + generate_powers(5, duration_btwn_points.seconds(), T); + + double start_pos = state_a.position; + double start_vel = state_a.velocity; + double start_acc = state_a.acceleration; + double end_pos = state_b.position; + double end_vel = state_b.velocity; + double end_acc = state_b.acceleration; + + double coefficients[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + coefficients[0] = start_pos; + coefficients[1] = start_vel; + coefficients[2] = 0.5 * start_acc; + if (duration_btwn_points.seconds() != 0.0) + { + coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + + end_acc * T[2] - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / + (2.0 * T[3]); + coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - + 2.0 * end_acc * T[2] + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / + (2.0 * T[4]); + coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - + 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / + (2.0 * T[5]); + } + + output.position = t[0] * coefficients[0] + t[1] * coefficients[1] + t[2] * coefficients[2] + + t[3] * coefficients[3] + t[4] * coefficients[4] + t[5] * coefficients[5]; + output.velocity = t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + + t[2] * 3.0 * coefficients[3] + t[3] * 4.0 * coefficients[4] + + t[4] * 5.0 * coefficients[5]; + output.acceleration = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3] + + t[2] * 12.0 * coefficients[4] + t[3] * 20.0 * coefficients[5]; + } + } + else + { + output.position = state_b.position; + output.velocity = state_b.velocity; + output.acceleration = state_b.acceleration; + } + + splines_state.position = output.position; + splines_state.velocity = output.velocity; + splines_state.acceleration = output.acceleration; + splines_state.effort = output.effort; + + return true; +} + +void Trajectory::deduce_from_derivatives( + control_msgs::msg::AxisTrajectoryPoint & first_state, + control_msgs::msg::AxisTrajectoryPoint & second_state, const double delta_t) +{ + if (std::isnan(second_state.position)) + { + if (std::isnan(first_state.velocity)) + { + first_state.velocity = 0; + } + if (std::isnan(second_state.velocity)) + { + if (std::isnan(first_state.acceleration)) + { + first_state.acceleration = 0; + } + second_state.velocity = + first_state.velocity + + (first_state.acceleration + second_state.acceleration) * 0.5 * delta_t; + } + second_state.position = + first_state.position + (first_state.velocity + second_state.velocity) * 0.5 * delta_t; + } +} + +TrajectoryPointConstIter Trajectory::begin(std::size_t axis_index) const +{ + THROW_ON_NULLPTR(trajectory_msg_) + + return trajectory_msg_->axis_trajectories[axis_index].axis_points.begin(); +} + +TrajectoryPointConstIter Trajectory::end(std::size_t axis_index) const +{ + THROW_ON_NULLPTR(trajectory_msg_) + + return trajectory_msg_->axis_trajectories[axis_index].axis_points.end(); +} + +rclcpp::Time Trajectory::start_time() const { return trajectory_start_time_; } + +bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } + +bool Trajectory::has_nontrivial_msg(std::size_t axis_index) const +{ + return has_trajectory_msg() && + trajectory_msg_->axis_trajectories[axis_index].axis_points.size() > 1; +} + +} // namespace multi_time_trajectory_controller diff --git a/multi_time_trajectory_controller/test/mac_visual_test.cpp b/multi_time_trajectory_controller/test/mac_visual_test.cpp new file mode 100644 index 0000000000..007e6ac325 --- /dev/null +++ b/multi_time_trajectory_controller/test/mac_visual_test.cpp @@ -0,0 +1,90 @@ +// 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 "lifecycle_msgs/msg/state.hpp" +#include "test_mttc_utils.hpp" + +#include + +using lifecycle_msgs::msg::State; + +namespace test_mttc +{ + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class MacVisualTest : public TrajectoryControllerTestParameterized +{ +public: + virtual void SetUp() + { + TrajectoryControllerTestParameterized::SetUp(); + // set up plotjuggler + } + + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } +}; +} // namespace test_mttc +using test_mttc::MacVisualTest; + +TEST_P(MacVisualTest, visual_test) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); + + std::size_t n_axes = axis_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_axes, state_reference.size()); + + // is the trajectory still active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should still hold the points from above + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_TRUE(traj_controller_->has_nontrivial_traj(i)); + } + + EXPECT_NEAR(state_reference[0].position, points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference[1].position, points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference[2].position, points.at(2).at(2), 1e-2); + // value of velocity is different from above due to spline interpolation + EXPECT_GT(state_reference[0].velocity, 0.0); + EXPECT_GT(state_reference[1].velocity, 0.0); + EXPECT_GT(state_reference[2].velocity, 0.0); + + executor.cancel(); +} diff --git a/multi_time_trajectory_controller/test/test_assets.hpp b/multi_time_trajectory_controller/test/test_assets.hpp new file mode 100644 index 0000000000..9f1f43b9b4 --- /dev/null +++ b/multi_time_trajectory_controller/test/test_assets.hpp @@ -0,0 +1,276 @@ +// 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. + +#ifndef TEST_ASSETS_HPP_ +#define TEST_ASSETS_HPP_ + +namespace test_mttc +{ +const auto urdf_rrrbot_revolute = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_rrrbot_continuous = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace test_mttc + +#endif // TEST_ASSETS_HPP_ diff --git a/multi_time_trajectory_controller/test/test_load_mttc.cpp b/multi_time_trajectory_controller/test/test_load_mttc.cpp new file mode 100644 index 0000000000..53dfc8e893 --- /dev/null +++ b/multi_time_trajectory_controller/test/test_load_mttc.cpp @@ -0,0 +1,42 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 "controller_manager/controller_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMAC, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_multi_axis_controller", + "multi_time_trajectory_controller/MultiTimeTrajectoryController"), + nullptr); + + rclcpp::shutdown(); +} diff --git a/multi_time_trajectory_controller/test/test_mttc.cpp b/multi_time_trajectory_controller/test/test_mttc.cpp new file mode 100644 index 0000000000..0f9a1f0bba --- /dev/null +++ b/multi_time_trajectory_controller/test/test_mttc.cpp @@ -0,0 +1,2578 @@ +// 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 +#include +#include +#include +#include + +#include +#include +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "control_msgs/msg/multi_axis_trajectory.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "multi_time_trajectory_controller/multi_time_trajectory_controller.hpp" +#include "test_mttc_utils.hpp" + +#include "test_assets.hpp" + +using lifecycle_msgs::msg::State; +using test_mttc::TrajectoryControllerTest; +using test_mttc::TrajectoryControllerTestParameterized; + +TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + traj_controller_->update( + rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); + + // hw position == 0 because controller is not activated + EXPECT_EQ(0.0, axis_pos_[0]); + EXPECT_EQ(0.0, axis_pos_[1]); + EXPECT_EQ(0.0, axis_pos_[2]); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, check_interface_names) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + compare_axes(axis_names_, axis_names_); +} + +TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_axes) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // set command_axes parameter different than axis_names_ + const rclcpp::Parameter command_axis_names_param("command_axes", command_axis_names_); + SetUpTrajectoryController(executor, {command_axis_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + compare_axes(axis_names_, command_axis_names_); +} + +TEST_P(TrajectoryControllerTestParameterized, activate) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), axis_names_.size() * command_interface_types_.size()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), axis_names_.size() * state_interface_types_.size()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, cleanup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + traj_controller_->update( + rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); + + auto state = traj_controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + state = traj_controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + // configure controller + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + // cleanup controller + state = traj_controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); + + // This call is replacing the way parameters are set via launch + auto state = traj_controller_->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + state = ActivateTrajectoryController(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(INITIAL_POS_AXIS1, axis_pos_[0]); + EXPECT_EQ(INITIAL_POS_AXIS2, axis_pos_[1]); + EXPECT_EQ(INITIAL_POS_AXIS3, axis_pos_[2]); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + // first update + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + + // wait for reaching the first point + // controller would process the second point when deactivated below + traj_controller_->update( + rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); + EXPECT_TRUE(traj_controller_->has_active_traj()); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(points.at(0).at(0), axis_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(1), axis_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(2), axis_pos_[2], COMMON_THRESHOLD); + } + + // deactivate + std::vector deactivated_positions{axis_pos_[0], axis_pos_[1], axis_pos_[2]}; + state = traj_controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + // it should be holding the current point + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectHoldingPointDeactivated(deactivated_positions); + + // reactivate + // wait so controller would have processed the third point when reactivated -> but it shouldn't + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + state = ActivateTrajectoryController(false, deactivated_positions); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); + + // it should still be holding the position at time of deactivation + // i.e., active but trivial trajectory (one point only) + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectCommandPoint(deactivated_positions); + + executor.cancel(); +} + +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + subscribeToState(executor); + updateController(); + + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + + ASSERT_FALSE(state == nullptr); + + std::size_t n_axes = axis_names_.size(); + + for (unsigned int i = 0; i < n_axes; ++i) + { + EXPECT_EQ(axis_names_[i], state->axis_names[i]); + } + + // No trajectory by default + EXPECT_TRUE( + (state->reference.axis_names.size() == 0) && (state->reference.axis_trajectories.size() == 0)); + + for (std::size_t i = 0; i < DOF; ++i) + { + EXPECT_EQ(state->error[i].position, 0); + EXPECT_TRUE(std::isnan(state->error[i].velocity) || state->error[i].velocity == 0); + EXPECT_TRUE(std::isnan(state->error[i].acceleration) || state->error[i].acceleration == 0); + + // expect feedback including all state_interfaces + EXPECT_EQ(n_axes, state->feedback.size()); + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + state_interface_types_.end()) + { + EXPECT_TRUE(std::isnan(state->feedback[i].velocity)); + } + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + state_interface_types_.end()) + { + EXPECT_TRUE(std::isnan(state->feedback[i].acceleration)); + } + + EXPECT_EQ(n_axes, state->output.size()); + + // expect output including all command_interfaces + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == + command_interface_types_.end()) + { + EXPECT_TRUE(std::isnan(state->output[i].position)); + } + + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == + command_interface_types_.end()) + { + EXPECT_TRUE(std::isnan(state->output[i].velocity)); + } + + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == + command_interface_types_.end()) + { + EXPECT_TRUE(std::isnan(state->output[i].acceleration)); + } + + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == + command_interface_types_.end()) + { + EXPECT_TRUE(std::isnan(state->output[i].effort)); + } + } +} + +/** + * @brief check if dynamic parameters are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateControllerAsync(); + auto pids = traj_controller_->get_pids(); + + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_EQ(pids.size(), 3); + auto gain_0 = pids.at(0)->get_gains(); + EXPECT_EQ(gain_0.p_gain_, 0.0); + + double kp = 1.0; + SetPidParameters(kp); + updateControllerAsync(); + + pids = traj_controller_->get_pids(); + EXPECT_EQ(pids.size(), 3); + gain_0 = pids.at(0)->get_gains(); + EXPECT_EQ(gain_0.p_gain_, kp); + } + else + { + // nothing to check here, skip further test + EXPECT_EQ(pids.size(), 0); + } + + executor.cancel(); +} + +/** + * @brief check if dynamic tolerances are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateControllerAsync(); + + // test default parameters + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 0.0); + for (std::size_t i = 0; i < axis_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01); + } + } + + // change parameters, update and see what happens + std::vector new_tolerances{ + rclcpp::Parameter("constraints.goal_time", 1.0), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02), + rclcpp::Parameter("constraints.axis1.trajectory", 1.0), + rclcpp::Parameter("constraints.axis2.trajectory", 2.0), + rclcpp::Parameter("constraints.axis3.trajectory", 3.0), + rclcpp::Parameter("constraints.axis1.goal", 10.0), + rclcpp::Parameter("constraints.axis2.goal", 20.0), + rclcpp::Parameter("constraints.axis3.goal", 30.0)}; + for (const auto & param : new_tolerances) + { + traj_controller_->get_node()->set_parameter(param); + } + updateControllerAsync(); + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 1.0); + for (std::size_t i = 0; i < axis_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast(i) + 1.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast(i) + 1.0)); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02); + } + } + + executor.cancel(); +} + +/** + * @brief check if hold on startup + */ +TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor, {}); + + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + // after startup, we expect an active trajectory: + ASSERT_TRUE(traj_controller_->has_active_traj()); + // one point, being the position at startup + std::vector initial_position{INITIAL_POS_AXIS1, INITIAL_POS_AXIS2, INITIAL_POS_AXIS3}; + + expectCommandPoint(initial_position); + + executor.cancel(); +} + +// Floating-point value comparison threshold +const double EPS = 1e-6; + +/** + * @brief check if calculated trajectory error is correct (angle wraparound) for continuous axes + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, INITIAL_POS_AXES, INITIAL_VEL_AXES, INITIAL_ACC_AXES, + INITIAL_EFF_AXES, test_mttc::urdf_rrrbot_continuous); + + std::size_t n_axes = axis_names_.size(); + + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_acceleration{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + std::size_t n_points = points.size(); + + for (std::size_t point_num = 0; point_num < n_points; ++point_num) + { + std::vector error, current, desired; + for (std::size_t axis = 0; axis < n_axes; ++axis) + { + control_msgs::msg::AxisTrajectoryPoint point; + point.position = points[point_num][axis]; + point.position = points_velocity[point_num][axis]; + point.position = points_acceleration[point_num][axis]; + current.push_back(point); + } + + // zero error + desired = current; + traj_controller_->testable_compute_error(error, current, desired); + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_NEAR(error[i].position, 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocity = desired.velocity - current.velocity; + EXPECT_NEAR(error[i].velocity, 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.acceleration = desired.acceleration - current.acceleration; + EXPECT_NEAR(error[i].acceleration, 0., EPS); + } + } + + // angle wraparound of position error + desired[0].position += 3.0 * M_PI_2; + desired[0].velocity += 1.0; + desired[0].acceleration += 1.0; + traj_controller_->testable_compute_error(error, current, desired); + for (std::size_t i = 0; i < n_axes; ++i) + { + if (i == 0) + { + EXPECT_NEAR(error[i].position, desired[i].position - current[i].position - 2.0 * M_PI, EPS); + } + else + { + EXPECT_NEAR(error[i].position, desired[i].position - current[i].position, EPS); + } + + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocity = desired.velocity - current.velocity; + EXPECT_NEAR(error[i].velocity, desired[i].velocity - current[i].velocity, EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.acceleration = desired.acceleration - current.acceleration; + EXPECT_NEAR(error[i].acceleration, desired[i].acceleration - current[i].acceleration, EPS); + } + } + } + executor.cancel(); +} + +/** + * @brief check if calculated trajectory error is correct (no angle wraparound) for revolute axes + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, INITIAL_POS_AXES, INITIAL_VEL_AXES, INITIAL_ACC_AXES, + INITIAL_EFF_AXES, test_mttc::urdf_rrrbot_revolute); + + std::size_t n_axes = axis_names_.size(); + + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_acceleration{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + std::size_t n_points = points.size(); + + for (std::size_t point_num = 0; point_num < n_points; ++point_num) + { + std::vector error, current, desired; + for (std::size_t axis = 0; axis < n_axes; ++axis) + { + control_msgs::msg::AxisTrajectoryPoint point; + point.position = points[point_num][axis]; + point.position = points_velocity[point_num][axis]; + point.position = points_acceleration[point_num][axis]; + current.push_back(point); + } + + // zero error + desired = current; + traj_controller_->testable_compute_error(error, current, desired); + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_NEAR(error[i].position, 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocity = desired.velocity - current.velocity; + EXPECT_NEAR(error[i].velocity, 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.acceleration = desired.acceleration - current.acceleration; + EXPECT_NEAR(error[i].acceleration, 0., EPS); + } + } + + // no normalization of position error + desired[0].position += 3.0 * M_PI_4; + desired[0].velocity += 1.0; + desired[0].acceleration += 1.0; + + traj_controller_->testable_compute_error(error, current, desired); + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_NEAR(error[i].position, desired[i].position - current[i].position, EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocity = desired.velocity - current.velocity; + EXPECT_NEAR(error[i].velocity, desired[i].velocity - current[i].velocity, EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.acceleration = desired.acceleration - current.acceleration; + EXPECT_NEAR(error[i].acceleration, desired[i].acceleration - current[i].acceleration, EPS); + } + } + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute axes aren't wrapped around (state topic) + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_AXES, INITIAL_VEL_AXES, INITIAL_ACC_AXES, + INITIAL_EFF_AXES, test_mttc::urdf_rrrbot_revolute); + subscribeToState(executor); + + std::size_t n_axes = axis_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time()); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // does the message have the correct sizes? + ASSERT_EQ(n_axes, state_reference.size()); + ASSERT_EQ(n_axes, state_feedback.size()); + ASSERT_EQ(n_axes, state_error.size()); + + for (std::size_t i = 0; i < n_axes; ++i) + { + // no update of state_interface + EXPECT_EQ(state_feedback[i].position, INITIAL_POS_AXES[i]); + } + + // are the correct reference position used? + EXPECT_NEAR(points[0][0], state_reference[0].position, COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference[1].position, COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference[2].position, COMMON_THRESHOLD); + + // no normalization of position error + EXPECT_NEAR(state_error[0].position, state_reference[0].position - INITIAL_POS_AXES[0], EPS); + EXPECT_NEAR(state_error[1].position, state_reference[1].position - INITIAL_POS_AXES[1], EPS); + EXPECT_NEAR(state_error[2].position, state_reference[2].position - INITIAL_POS_AXES[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], axis_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], axis_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], axis_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for position + EXPECT_NEAR( + k_p * (state_reference[0].position - INITIAL_POS_AXES[0]), axis_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference[1].position - INITIAL_POS_AXES[1]), axis_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference[2].position - INITIAL_POS_AXES[2]), axis_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocity only + // check command interface + EXPECT_LT(0.0, axis_vel_[0]); + EXPECT_LT(0.0, axis_vel_[1]); + EXPECT_LT(0.0, axis_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) for position + EXPECT_NEAR( + k_p * (state_reference[0].position - INITIAL_POS_AXES[0]), axis_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference[1].position - INITIAL_POS_AXES[1]), axis_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference[2].position - INITIAL_POS_AXES[2]), axis_eff_[2], + k_p * COMMON_THRESHOLD); + } + + executor.cancel(); +} + +/** + * @brief check if position error of continuous axes are wrapped around (state topic) + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_AXES, INITIAL_VEL_AXES, INITIAL_ACC_AXES, + INITIAL_EFF_AXES, test_mttc::urdf_rrrbot_continuous); + + std::size_t n_axes = axis_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + for (std::size_t axis = 0; axis < n_axes; ++axis) + { + EXPECT_EQ(state_feedback[axis].position, INITIAL_POS_AXES[axis]); + } + + // has the msg the correct vector sizes? + ASSERT_EQ(n_axes, state_reference.size()); + ASSERT_EQ(n_axes, state_feedback.size()); + ASSERT_EQ(n_axes, state_error.size()); + + // are the correct reference position used? + EXPECT_NEAR(points[0][0], state_reference[0].position, COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference[1].position, COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference[2].position, COMMON_THRESHOLD); + + // is error.position[2] wrapped around? + EXPECT_NEAR(state_error[0].position, state_reference[0].position - INITIAL_POS_AXES[0], EPS); + EXPECT_NEAR(state_error[1].position, state_reference[1].position - INITIAL_POS_AXES[1], EPS); + EXPECT_NEAR( + state_error[2].position, state_reference[2].position - INITIAL_POS_AXES[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], axis_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], axis_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], axis_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for axis0 and axis1 + EXPECT_NEAR( + k_p * (state_reference[0].position - INITIAL_POS_AXES[0]), axis_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference[1].position - INITIAL_POS_AXES[1]), axis_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of position[2] wrapped around? + EXPECT_GT(0.0, axis_vel_[2]); // direction change because of angle wrap + EXPECT_NEAR( + k_p * (state_reference[2].position - INITIAL_POS_AXES[2] - 2 * M_PI), axis_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocity only + // check command interface + EXPECT_LT(0.0, axis_vel_[0]); + EXPECT_LT(0.0, axis_vel_[1]); + EXPECT_LT(0.0, axis_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) for axis0 and axis1 + EXPECT_NEAR( + k_p * (state_reference[0].position - INITIAL_POS_AXES[0]), axis_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference[1].position - INITIAL_POS_AXES[1]), axis_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of position[2] wrapped around? + EXPECT_GT(0.0, axis_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference[2].position - INITIAL_POS_AXES[2] - 2 * M_PI), axis_eff_[2], + k_p * COMMON_THRESHOLD); + } + + executor.cancel(); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, accept_correct_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + double cmd_timeout = 3.0; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(cmd_timeout, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 1.0); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(0.0, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief check if no timeout is triggered + */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync +TEST_P(TrajectoryControllerTestParameterized, no_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); + + std::size_t n_axes = axis_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_axes, state_reference.size()); + + // is the trajectory still active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should still hold the points from above + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_TRUE(traj_controller_->has_nontrivial_traj(i)); + } + + EXPECT_NEAR(state_reference[0].position, points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference[1].position, points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference[2].position, points.at(2).at(2), 1e-2); + // value of velocity is different from above due to spline interpolation + EXPECT_GT(state_reference[0].velocity, 0.0); + EXPECT_GT(state_reference[1].velocity, 0.0); + EXPECT_GT(state_reference[2].velocity, 0.0); + + executor.cancel(); +} + +/** + * @brief check if timeout is triggered + */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync +TEST_P(TrajectoryControllerTestParameterized, timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double cmd_timeout = 0.1; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + // update until end of trajectory -> no timeout should have occurred + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3); + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should have the trajectory with three points + std::size_t n_axes = axis_names_.size(); + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_TRUE(traj_controller_->has_nontrivial_traj(i)); + } + + // update until timeout should have happened + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // after timeout, set_hold_position adds a new trajectory + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should be not more than one point now (from hold position) + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_FALSE(traj_controller_->has_nontrivial_traj(i)); + } + // should hold last position with zero velocity + if (traj_controller_->has_position_command_interface()) + { + expectCommandPoint(points.at(2)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectCommandPoint(INITIAL_POS_AXES); + } + + executor.cancel(); +} + +/** + * @brief check if use_closed_loop_pid is active + */ +TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + if ( + (traj_controller_->has_velocity_command_interface() && + !traj_controller_->has_position_command_interface() && + !traj_controller_->has_effort_command_interface() && + !traj_controller_->has_acceleration_command_interface() && + !traj_controller_->is_open_loop()) || + traj_controller_->has_effort_command_interface()) + { + EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + } +} + +/** + * @brief check if velocity error is calculated correctly + */ +TEST_P(TrajectoryControllerTestParameterized, velocity_error) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}, true); + + std::size_t n_axes = axis_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points_position{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.1, 0.1, 0.1}}, {{0.2, 0.2, 0.2}}, {{0.3, 0.3, 0.3}}}; + // *INDENT-ON* + publish(time_from_start, points_position, rclcpp::Time(), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_axes, state_reference.size()); + EXPECT_EQ(n_axes, state_feedback.size()); + EXPECT_EQ(n_axes, state_error.size()); + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(n_axes, state_reference.size()); + EXPECT_EQ(n_axes, state_feedback.size()); + EXPECT_EQ(n_axes, state_error.size()); + } + + // no change in state interface should happen + if (traj_controller_->has_velocity_state_interface()) + { + for (std::size_t i = 0; i < n_axes; ++i) + { + EXPECT_EQ(state_feedback[i].velocity, INITIAL_VEL_AXES[i]); + } + } + // is the velocity error correct? + if ( + traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller + || (traj_controller_->has_velocity_state_interface() && + traj_controller_->has_velocity_command_interface())) + { + // don't check against a value, because spline interpolation might overshoot depending on + // interface combinations + EXPECT_GE(state_error[0].velocity, points_velocity[0][0]); + EXPECT_GE(state_error[1].velocity, points_velocity[0][1]); + EXPECT_GE(state_error[2].velocity, points_velocity[0][2]); + } + + executor.cancel(); +} + +/** + * @brief test_jumbled_axis_order Test sending trajectories with a axis order different from + * internal controller order + */ +TEST_P(TrajectoryControllerTestParameterized, test_jumbled_axis_order) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor); + std::vector points_position = {1.0, 2.0, 3.0}; + std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; + { + control_msgs::msg::MultiAxisTrajectory traj_msg; + const std::vector jumbled_axis_names{ + axis_names_[jumble_map[0]], axis_names_[jumble_map[1]], axis_names_[jumble_map[2]]}; + + traj_msg.axis_names = jumbled_axis_names; + traj_msg.header.stamp = rclcpp::Time(0); + + std::size_t n_axes = axis_names_.size(); + traj_msg.axis_trajectories.resize(n_axes); + for (std::size_t i = 0; i < n_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points.resize(1); + traj_msg.axis_trajectories[i].axis_points[0].time_from_start = + rclcpp::Duration::from_seconds(dt); + traj_msg.axis_trajectories[i].axis_points[0].position = points_position.at(jumble_map[i]); + traj_msg.axis_trajectories[i].axis_points[0].velocity = + (traj_msg.axis_trajectories[i].axis_points[0].position - axis_pos_[jumble_map[i]]) / dt; + traj_msg.axis_trajectories[i].axis_points[0].acceleration = + (traj_msg.axis_trajectories[i].axis_points[0].velocity - axis_vel_[jumble_map[i]]) / dt; + } + + trajectory_publisher_->publish(traj_msg); + } + + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(points_position.at(0), axis_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points_position.at(1), axis_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points_position.at(2), axis_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // if use_closed_loop_pid_adapter_==false: we expect desired velocity from direct sampling + // if use_closed_loop_pid_adapter_==true: we expect desired velocity, because we use PID with + // feedforward term only + EXPECT_GT(0.0, axis_vel_[0]); + EXPECT_GT(0.0, axis_vel_[1]); + EXPECT_GT(0.0, axis_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_GT(0.0, axis_acc_[0]); + EXPECT_GT(0.0, axis_acc_[1]); + EXPECT_GT(0.0, axis_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + // effort should be nonzero, because we use PID with feedforward term + EXPECT_GT(0.0, axis_eff_[0]); + EXPECT_GT(0.0, axis_eff_[1]); + EXPECT_GT(0.0, axis_eff_[2]); + } +} + +/** + * @brief invalid_message Test mismatched axis and reference vector sizes + */ +TEST_P(TrajectoryControllerTestParameterized, invalid_message) +{ + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters}); + + control_msgs::msg::MultiAxisTrajectory traj_msg, good_traj_msg; + + good_traj_msg.axis_names = axis_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + std::size_t const num_axes = 3; + good_traj_msg.axis_trajectories.resize(num_axes); + + std::vector const positions = {1.0, 2.0, 3.0}; + std::vector const velocities = {-1.0, -2.0, -3.0}; + for (std::size_t i = 0; i < num_axes; ++i) + { + good_traj_msg.axis_trajectories[i].axis_points.resize(1); + good_traj_msg.axis_trajectories[i].axis_points[0].time_from_start = + rclcpp::Duration::from_seconds(0.25); + good_traj_msg.axis_trajectories[i].axis_points[0].position = positions[i]; + good_traj_msg.axis_trajectories[i].axis_points[0].velocity = velocities[i]; + } + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // Incompatible axis names + traj_msg = good_traj_msg; + traj_msg.axis_names = {"bad_name"}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // empty message + traj_msg = good_traj_msg; + traj_msg.axis_trajectories.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position data + traj_msg = good_traj_msg; + for (std::size_t i = 0; i < num_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points[0].position = + std::numeric_limits::quiet_NaN(); + } + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Non-monotonically increasing waypoint times + traj_msg = good_traj_msg; + for (std::size_t i = 0; i < num_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points.push_back( + traj_msg.axis_trajectories[i].axis_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}); + + control_msgs::msg::MultiAxisTrajectory traj_msg; + traj_msg.axis_names = axis_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! + std::size_t num_axes = axis_names_.size(); + traj_msg.axis_trajectories.resize(num_axes); + std::vector const positions = {1.0, 2.0, 3.0}; + std::vector const velocities = {-1.0, -2.0, -3.0}; + for (std::size_t i = 0; i < num_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points.resize(1); + traj_msg.axis_trajectories[i].axis_points[0].time_from_start = + rclcpp::Duration::from_seconds(0.25); + traj_msg.axis_trajectories[i].axis_points[0].position = positions[i]; + traj_msg.axis_trajectories[i].axis_points[0].velocity = velocities[i]; + } + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief missing_position_message_accepted Test mismatched axis and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocity are accepted + */ +TEST_P(TrajectoryControllerTestParameterized, missing_position_message_accepted) +{ + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters}); + + control_msgs::msg::MultiAxisTrajectory traj_msg, good_traj_msg; + + good_traj_msg.axis_names = axis_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + std::size_t const num_axes = axis_names_.size(); + good_traj_msg.axis_trajectories.resize(num_axes); + std::vector const positions = {1.0, 2.0, 3.0}; + std::vector const velocities = {-1.0, -2.0, -3.0}; + for (std::size_t i = 0; i < num_axes; ++i) + { + good_traj_msg.axis_trajectories[i].axis_points.resize(1); + good_traj_msg.axis_trajectories[i].axis_points[0].time_from_start = + rclcpp::Duration::from_seconds(0.25); + good_traj_msg.axis_trajectories[i].axis_points[0].position = positions[i]; + good_traj_msg.axis_trajectories[i].axis_points[0].velocity = velocities[i]; + } + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // No position data + traj_msg = good_traj_msg; + for (std::size_t i = 0; i < num_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points[0].position = + std::numeric_limits::quiet_NaN(); + } + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position and velocity data + traj_msg = good_traj_msg; + for (std::size_t i = 0; i < num_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points[0].position = + std::numeric_limits::quiet_NaN(); + traj_msg.axis_trajectories[i].axis_points[0].velocity = + std::numeric_limits::quiet_NaN(); + } + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // All empty + traj_msg = good_traj_msg; + for (std::size_t i = 0; i < num_axes; ++i) + { + traj_msg.axis_trajectories[i].axis_points[0].position = + std::numeric_limits::quiet_NaN(); + traj_msg.axis_trajectories[i].axis_points[0].velocity = + std::numeric_limits::quiet_NaN(); + traj_msg.axis_trajectories[i].axis_points[0].acceleration = + std::numeric_limits::quiet_NaN(); + } + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief test_trajectory_replace Test replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + + std::vector> positions_old{{2., 3., 4.}}; + std::vector> velocities_old{{0.2, 0.3, 0.4}}; + std::vector> positions_new_partial{{1.5}}; + std::vector> velocities_new_partial{{0.15}}; + + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, positions_old, rclcpp::Time(), {}, velocities_old); + std::vector expected_actual, expected_desired; + std::size_t num_axes = positions_old[0].size(); + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + expected_desired.resize(num_axes); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = positions_old[0][i]; + expected_desired[i].position = positions_old[0][i]; + expected_actual[i].velocity = velocities_old[0][i]; + expected_desired[i].velocity = velocities_old[0][i]; + } + // Check that we reached end of points_old trajectory + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); + velocities_new_partial[0][0] = + std::copysign(0.15, positions_new_partial[0][0] - axis_state_pos_[0]); + publish(time_from_start, positions_new_partial, rclcpp::Time(), {}, velocities_new_partial); + + // Replaced trajectory is a mix of previous and current goal + expected_desired[0].position = positions_new_partial[0][0]; + expected_desired[1].position = positions_old[0][1]; + expected_desired[2].position = positions_old[0][2]; + expected_desired[0].velocity = velocities_new_partial[0][0]; + expected_desired[1].velocity = 0.0; + expected_desired[2].velocity = 0.0; + expected_actual = expected_desired; + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); +} + +/** + * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + + // TODO(anyone): add expectations for velocity and acceleration + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}}}; + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time()); + + // a vector of axis states at each time + std::vector expected_actuals, expected_desireds; + std::size_t num_axes = points_old[0].size(); + expected_actuals.resize(num_axes); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actuals[i].position = points_old[0][i]; + } + expected_desireds = expected_actuals; + // Check that we reached end of points_old[0] trajectory + auto end_time = waitAndCompareState( + expected_actuals, expected_desireds, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); + // New trajectory will end before current time + rclcpp::Time new_traj_start = + rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actuals[i].position = points_old[1][i]; + } + expected_desireds = expected_actuals; + publish(time_from_start, points_new, new_traj_start); + waitAndCompareState( + expected_actuals, expected_desireds, executor, rclcpp::Duration(delay), 0.1, end_time); +} + +TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + std::vector expected_actual, expected_desired; + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + + // send points_old and wait to reach first point + publish(time_from_start, points_old, rclcpp::Time()); + std::size_t num_axes = points_old.size(); + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = points_old[0][i]; + } + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + // send points_new before the old trajectory is finished + RCLCPP_INFO( + traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); + // New trajectory first point is in the past, second is in the future + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); + publish(time_from_start, points_new, new_traj_start); + // it should not have accepted the new goal but finish the old one + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = points_old[1][i]; + } + expected_desired = expected_actual; + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); +} + +TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + + RCLCPP_WARN( + traj_controller_->get_node()->get_logger(), + "Test disabled until current_trajectory is taken into account when adding a new trajectory."); + return; + + // *INDENT-OFF* + std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> full_traj_velocity{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; + std::vector> partial_traj{{{-1., -2.}, {-2., -4}}}; + std::vector> partial_traj_velocity{{{-0.1, -0.2}, {-0.2, -0.4}}}; + // *INDENT-ON* + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; + // Send full trajectory + publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocity); + // Sleep until first waypoint of full trajectory + + std::vector expected_actual, expected_desired; + std::size_t num_axes = full_traj[0].size(); + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = full_traj[0][i]; + } + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory and are starting points_old[1] + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + // Send partial trajectory starting after full trajecotry is complete + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + publish( + points_delay, partial_traj, rclcpp::Clock(RCL_STEADY_TIME).now() + delay * 2, {}, + partial_traj_velocity); + // Wait until the end start and end of partial traj + + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = partial_traj.back()[i]; + } + expected_desired = expected_actual; + + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); +} + +TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set parameter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector> first_goal_velocity = {{0.33, 0.44, 0.55}}; + std::vector second_goal = {6.6, 8.8, 11.0}; + std::vector> second_goal_velocity = {{0.66, 0.88, 1.1}}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); + std::vector> points{{first_goal}}; + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocity); + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); + + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_AXIS1, axis_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], axis_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + axis_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move axis further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocity); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], axis_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance + EXPECT_NEAR( + axis_state_pos_[0] + (second_goal[0] - axis_state_pos_[0]) * trajectory_frac, axis_pos_[0], + 0.1); + EXPECT_GT(axis_pos_[0], axis_state_pos_[0]); + EXPECT_LT(axis_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(axis_pos_[0], axis_state_pos_[0]); + EXPECT_LT(axis_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(axis_pos_[0], axis_state_pos_[0]); + EXPECT_LT(axis_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], axis_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + axis_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move axis back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], axis_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + axis_state_pos_[0] + (first_goal[0] - axis_state_pos_[0]) * trajectory_frac, axis_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(axis_pos_[0], axis_state_pos_[0]); + EXPECT_GT(axis_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(axis_pos_[0], axis_state_pos_[0]); + EXPECT_GT(axis_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(axis_pos_[0], axis_state_pos_[0]); + EXPECT_GT(axis_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], axis_pos_[0], COMMON_THRESHOLD); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // set open loop to true, this should change behavior from above + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector second_goal = {6.6, 8.8, 11.0}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); + std::vector> points{{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); + + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_AXIS1, axis_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], axis_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + axis_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move axis further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], axis_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, axis_pos_[0], + COMMON_THRESHOLD); + EXPECT_GT(axis_pos_[0], first_goal[0]); + EXPECT_LT(axis_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(axis_pos_[0], first_goal[0]); + EXPECT_LT(axis_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(axis_pos_[0], first_goal[0]); + EXPECT_LT(axis_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], axis_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + axis_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move axis back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], axis_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, axis_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(axis_pos_[0], second_goal[0]); + EXPECT_GT(axis_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(axis_pos_[0], first_goal[0]); + EXPECT_LT(axis_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(axis_pos_[0], first_goal[0]); + EXPECT_LT(axis_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], axis_pos_[0], COMMON_THRESHOLD); + + executor.cancel(); +} + +// Testing that values are read from state interfaces when hardware is started for the first +// time and hardware state has offset --> this is indicated by NaN values in command interfaces +TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to NaN + std::vector initial_pos_cmd{ + 3, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; + std::vector initial_vel_cmd{ + 3, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; + std::vector initial_acc_cmd{ + 3, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; + + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from state interfaces + // (command interface are NaN) + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + std::size_t num_axes = initial_pos_cmd.size(); + + for (std::size_t i = 0; i < num_axes; ++i) + { + EXPECT_EQ(current_state_when_offset[i].position, axis_state_pos_[i]); + + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(current_state_when_offset[i].velocity, axis_state_vel_[i]); + } + + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(current_state_when_offset[i].acceleration, axis_state_acc_[i]); + } + } + + executor.cancel(); +} + +// Testing that values are read from command interfaces when hardware is started after some values +// are set on the hardware commands +TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to arbitrary values + std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; + std::size_t num_axes = 3; + for (std::size_t i = 0; i < num_axes; ++i) + { + initial_pos_cmd.push_back(3.1 + static_cast(i)); + initial_vel_cmd.push_back(0.25 + static_cast(i)); + initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); + } + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from command interfaces + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + for (std::size_t i = 0; i < num_axes; ++i) + { + // check position + if (traj_controller_->has_position_command_interface()) + { + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + if (traj_controller_->has_velocity_command_interface()) + { + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + if (traj_controller_->has_acceleration_command_interface()) + { + // should have set it to last position + velocity + acceleration command + EXPECT_EQ(current_state_when_offset[i].position, initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset[i].velocity, initial_vel_cmd[i]); + EXPECT_EQ(current_state_when_offset[i].acceleration, initial_acc_cmd[i]); + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset[i].position, axis_state_pos_[i]); + EXPECT_EQ(current_state_when_offset[i].velocity, axis_state_vel_[i]); + EXPECT_EQ(current_state_when_offset[i].acceleration, axis_state_acc_[i]); + } + } + else + { + // should have set it to last position + velocity command + EXPECT_EQ(current_state_when_offset[i].position, initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset[i].velocity, initial_vel_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset[i].position, axis_state_pos_[i]); + EXPECT_EQ(current_state_when_offset[i].velocity, axis_state_vel_[i]); + } + } + else + { + // should have set it to last position command + EXPECT_EQ(current_state_when_offset[i].position, initial_pos_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset[i].position, axis_state_pos_[i]); + } + } + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) +{ + // set axis tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.axis1.trajectory", state_tol), + rclcpp::Parameter("constraints.axis2.trajectory", state_tol), + rclcpp::Parameter("constraints.axis3.trajectory", state_tol)}; + + rclcpp::executors::MultiThreadedExecutor executor; + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, params, true, kp); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectCommandPoint(axis_state_pos_); +} + +TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) +{ + // set axis tolerance parameters + const double goal_tol = 0.1; + // set very small goal_time so that goal_time is violated + const double goal_time = 0.000001; + std::vector params = { + rclcpp::Parameter("constraints.axis1.goal", goal_tol), + rclcpp::Parameter("constraints.axis2.goal", goal_tol), + rclcpp::Parameter("constraints.axis3.goal", goal_tol), + rclcpp::Parameter("constraints.goal_time", goal_time)}; + + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, params, true, 1.0); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocity{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocity); + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectCommandPoint(axis_state_pos_); + + // what happens if we wait longer but it harms the tolerance again? + auto hold_position = axis_state_pos_; + axis_state_pos_.at(0) = -3.3; + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); + // it should be still holding the old point + expectCommandPoint(hold_position); +} + +// position controllers +INSTANTIATE_TEST_SUITE_P( + PositionTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity_acceleration controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); + +/** + * @brief see if parameter validation is correct + * + * Note: generate_parameter_library validates parameters itself during on_init() method, but + * combinations of parameters are checked from JTC during on_configure() + */ +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ + // command interfaces: empty + command_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + auto state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + + // command interfaces: bad_name + command_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: effort has to be only + command_interface_types_ = {"effort", "position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: velocity - position not present + command_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: acceleration without position and velocity + command_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: empty + state_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: cannot not be effort + state_interface_types_ = {"effort"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: bad name + state_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: velocity - position not present + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + state_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: acceleration without position and velocity + state_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); +} + +TEST_F(TrajectoryControllerTest, open_closed_enable_disable) +{ + axis_names_ = {"x", "y", "z", "roll", "pitch", "yaw"}; + command_axis_names_ = {"command_x", "command_y", "command_z", + "command_roll", "command_pitch", "command_yaw"}; + command_interface_types_ = {"position", "velocity"}; + state_interface_types_ = {}; + axis_pos_.resize(axis_names_.size(), 0.0); + axis_state_pos_.resize(axis_names_.size(), 0.0); + axis_vel_.resize(axis_names_.size(), 0.0); + axis_state_vel_.resize(axis_names_.size(), 0.0); + axis_acc_.resize(axis_names_.size(), 0.0); + axis_state_acc_.resize(axis_names_.size(), 0.0); + axis_eff_.resize(axis_names_.size(), 0.0); + std::size_t const num_axes = 6; + + // set up controller into open loop mode + rclcpp::executors::SingleThreadedExecutor executor; + + std::vector params = { + {"open_loop_control", true}, + {"use_feedback", true}, + {"allow_integration_in_goal_trajectories", true}, + {"hold_last_velocity", true}}; + + SetUpTrajectoryController(executor, params); + traj_controller_->get_node()->configure(); + for (std::size_t i = 0; i < 2; ++i) + { + publish_feedback({0, 0, 0}, {1, 0, 0, 0}, {0, 0, 0}, {0, 0, 0}); + executor.spin_some(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // set all initial states to 6-vector of zeros + std::vector zeros(6, 0); + ActivateTrajectoryController(true, zeros, zeros, zeros, zeros); + + // [axis-mult] At 20 Hz, sends a 'reference' command with all zeros and time from start of 50ms + // (i.e. positions are NaN, velocities are zero and accelerations are NaN) + + constexpr std::size_t freq_Hz = 20; + std::size_t const ns_dur = 1000000000 / freq_Hz; + auto const chrono_duration = std::chrono::nanoseconds(ns_dur); + rclcpp::Duration const dur(0, ns_dur); + double nan = std::numeric_limits::quiet_NaN(); + std::vector point_nan(num_axes, nan); + std::vector point_zero(num_axes, 0); + + // start with zero vels and nan positions for 1 second + std::vector> positions(freq_Hz, point_nan); + std::vector> velocities(freq_Hz, point_zero); + + std::vector expected_actual; + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + std::vector expected_desired; + expected_desired.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = 0; + expected_actual[i].velocity = 0; + } + + expected_desired = expected_actual; + + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + traj_controller_->wait_for_trajectory(executor); + + // now test that we haven't moved + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // [axis-mult] When joystick is moved, it sends 'reference' command with non-zero velocities for + // axes moved in joystick. As we are in open-loop teleoperation, this command works + + positions = {freq_Hz, point_nan}; + + // 0.5 seconds of constant accel + for (std::size_t i = 0; i < freq_Hz / 2; ++i) + { + double target_vel_current = static_cast(i + 1); + // each axis's target velocity is proportional to time, which should give a constant accel + // set angular velocity target to 0 to avoid having to deal with rotating frame in this test + velocities.push_back( + {0.1 * target_vel_current, 0.2 * target_vel_current, 0.3 * target_vel_current, 0, 0, 0}); + } + + // then 0.5 seconds of constant vel + auto final_vel = velocities.back(); + for (std::size_t i = 0; i < freq_Hz / 2; ++i) + { + velocities.push_back(final_vel); + } + + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + // the final position should be the integral of the velocity profile we gave + // for constant accel portion, that is 0.5 * (0.5 seconds) * final_vel + // for constant vel portion, that is just 0.5 seconds * final_vel + expected_actual[i].position = 0.5 * 0.5 * final_vel[i] + 0.5 * final_vel[i]; + expected_actual[i].velocity = final_vel[i]; + } + + expected_desired = expected_actual; + + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // [axis-mult] Joystick is released and it continually sends a 'reference' command with all zeros + // and time from start of 50ms(i.e. positions are NaN, velocities are zero and accelerations are + // NaN) + + positions = {freq_Hz, point_nan}; + velocities = {freq_Hz, point_zero}; + + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + + // store the final position + auto final_pos = axis_pos_; + + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = final_pos[i]; + expected_actual[i].velocity = 0; + } + + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // store the new final position + final_pos = axis_pos_; + + // [external] Closed-loop control is enabled on X-Y i.e. on position and velocity controllers + // [axis-mult] Detects closed loop control is enabled for x-y axes + // [axis-mult] Sends a 'reset dofs' service call to CTG/MAC to reset x-y position to values + // specified in service call(values are current x-y state estimate) + + auto request = std::make_shared(); + + // reset x and y axes + // test that we can reset to different values + final_pos[0] = 3; + final_pos[1] = -1; + + request->names = {axis_names_[0], axis_names_[1]}; + request->positions = {final_pos[0], final_pos[1]}; + request->velocities = {0, 0}; + request->accelerations = {0, 0}; + send_reset_request(request, executor); + + // now axis_multiplexer sends all nans for x and y + positions = {freq_Hz, final_pos}; + velocities = {freq_Hz, zeros}; + + for (std::size_t i = 0; i < freq_Hz; ++i) + { + positions[i][0] = std::numeric_limits::quiet_NaN(); + positions[i][1] = std::numeric_limits::quiet_NaN(); + velocities[i][0] = std::numeric_limits::quiet_NaN(); + velocities[i][1] = std::numeric_limits::quiet_NaN(); + } + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities, true); + + // expect that we haven't moved from final_pos and that we still aren't moving + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = final_pos[i]; + expected_actual[i].velocity = 0; + } + expected_desired = expected_actual; + + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * (3 * freq_Hz / 2), 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // [axis-mult] When service request is completed, sends a 'reference_reliable' command to CTG/MAC + // with current x-y state estimate with 'time from start' set to 10ms (essentially a command to + // hold current position to be sent to closed-loop position controller). The velocities are set to + // zero and the accelerations to NaN in this command. + + // [axis-mult] At 20 Hz, continues sending 'reference' command of zeroes at 50ms + + positions = {freq_Hz, final_pos}; + velocities = {freq_Hz, zeros}; + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities, true, true); + + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = final_pos[i]; + expected_actual[i].velocity = 0; + } + expected_desired = expected_actual; + + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * (3 * freq_Hz / 2), 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // [axis-mult] When joystick is moved in X-Y to perform closed-loop teleoperation, it sends + // 'reference' command with non-zero velocities for axes moved in joystick. This command doesn't + // work on MAC but does on CTG. + + positions = {freq_Hz, point_nan}; + double const target_x_vel = 0.3142; + double const target_y_vel = -0.2718; + for (std::size_t i = 0; i < freq_Hz; ++i) + { + velocities.push_back({target_x_vel, target_y_vel, 0, 0, 0, 0}); + } + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities, true, false); + + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = final_pos[i]; + expected_actual[i].velocity = 0; + } + + expected_actual[0].position += target_x_vel; + expected_actual[1].position += target_y_vel; + + expected_actual[0].velocity = target_x_vel; + expected_actual[1].velocity = target_y_vel; + + expected_desired = expected_actual; + + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // If I disable closed-loop control for X-Y, and try to move X-Y in + // open-loop with joystick again on MAC, it doesn't move. If I move the joystick in a different + // axes e.g. heading, it works fine. I have to confirm again but Z doesn't work anymore though + // either on MAC. +} + +TEST_F(TrajectoryControllerTest, test_joint_limiter_active_but_no_joint_limiting) +{ + axis_names_ = {"x", "y", "z", "roll", "pitch", "yaw"}; + command_axis_names_ = {"command_x", "command_y", "command_z", + "command_roll", "command_pitch", "command_yaw"}; + command_interface_types_ = {"position", "velocity"}; + state_interface_types_ = {}; + axis_pos_.resize(axis_names_.size(), 0.0); + axis_state_pos_.resize(axis_names_.size(), 0.0); + axis_vel_.resize(axis_names_.size(), 0.0); + axis_state_vel_.resize(axis_names_.size(), 0.0); + axis_acc_.resize(axis_names_.size(), 0.0); + axis_state_acc_.resize(axis_names_.size(), 0.0); + axis_eff_.resize(axis_names_.size(), 0.0); + std::size_t const num_axes = 6; + + // set up controller into open loop mode + rclcpp::executors::SingleThreadedExecutor executor; + + std::vector params = { + {"open_loop_control", true}, + {"use_feedback", true}, + {"allow_integration_in_goal_trajectories", true}, + {"hold_last_velocity", true}, + {"joint_limiter_type", "joint_limits/JointSaturationLimiter"}, + // joint limits for x + {"joint_limits.x.has_position_limits", false}, + {"joint_limits.x.has_velocity_limits", true}, + {"joint_limits.x.max_velocity", 1.0}, + {"joint_limits.x.has_acceleration_limits", true}, + {"joint_limits.x.max_acceleration", 1.0}, + {"joint_limits.x.has_deceleration_limits", true}, + {"joint_limits.x.max_deceleration", 1.0}, + // joint limits for y + {"joint_limits.y.has_position_limits", false}, + {"joint_limits.y.has_velocity_limits", true}, + {"joint_limits.y.max_velocity", 2.0}, + {"joint_limits.y.has_acceleration_limits", true}, + {"joint_limits.y.max_acceleration", 2.0}, + {"joint_limits.y.has_deceleration_limits", true}, + {"joint_limits.y.max_deceleration", 1.0}, + }; + std::vector vel_limits = {1.0, 2.0, 1.0, + 1.0, 1.0, 1.0}; // these should match limits above + std::vector acc_limits = {1.0, 2.0, 1.0, + 1.0, 1.0, 1.0}; // these should match limits above + + SetUpTrajectoryController(executor, params); + traj_controller_->get_node()->configure(); + for (std::size_t i = 0; i < 2; ++i) + { + publish_feedback({0, 0, 0}, {1, 0, 0, 0}, {0, 0, 0}, {0, 0, 0}); + executor.spin_some(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // set all initial states to 6-vector of zeros + std::vector zeros(6, 0); + ActivateTrajectoryController(true, zeros, zeros, zeros, zeros); + + // [axis-mult] At 20 Hz, sends a 'reference' command with all zeros and time from start of 50ms + // (i.e. positions are NaN, velocities are zero and accelerations are NaN) + + constexpr std::size_t freq_Hz = 20; + std::size_t const ns_dur = 1000000000 / freq_Hz; + auto const chrono_duration = std::chrono::nanoseconds(ns_dur); + rclcpp::Duration const dur(0, ns_dur); + double nan = std::numeric_limits::quiet_NaN(); + std::vector point_nan(num_axes, nan); + std::vector point_zero(num_axes, 0); + + // start with zero vels and nan positions for 1 second + std::vector> positions(freq_Hz, point_nan); + std::vector> velocities(freq_Hz, point_zero); + + std::vector expected_actual; + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + std::vector expected_desired; + expected_desired.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = 0; + expected_actual[i].velocity = 0; + } + + expected_desired = expected_actual; + + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + traj_controller_->wait_for_trajectory(executor); + + // now test that we haven't moved + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // send 'reference' command with velocities that don't exceed limits and verify output is not + // limited + + positions = {freq_Hz, point_nan}; + + // 0.5 seconds of constant accel + for (std::size_t i = 0; i < freq_Hz / 2; ++i) + { + double target_vel_current = static_cast(i + 1); + // each axis's target velocity is proportional to time, which should give a constant accel + // set angular velocity target to 0 to avoid having to deal with rotating frame in this test + // Here we set the final velocity to be 0.1 for x and 0.2 for y (which is less than the limits) + velocities.push_back( + {0.1 * 2 * target_vel_current / freq_Hz, 0.2 * 2 * target_vel_current / freq_Hz, 0, 0, 0, 0}); + } + + // then 0.5 seconds of constant vel + auto final_vel = velocities.back(); + for (std::size_t i = 0; i < freq_Hz / 2; ++i) + { + velocities.push_back(final_vel); + } + + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + // the final position should be the integral of the velocity profile we gave + // for constant accel portion, that is 0.5 * (0.5 seconds) * final_vel + // for constant vel portion, that is just 0.5 seconds * final_vel + expected_actual[i].position = 0.5 * 0.5 * final_vel[i] + 0.5 * final_vel[i]; + expected_actual[i].velocity = final_vel[i]; + } + + expected_desired = expected_actual; + + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); +} + +TEST_F(TrajectoryControllerTest, test_joint_limiter_active_and_joint_limiting) +{ + axis_names_ = {"x", "y", "z", "roll", "pitch", "yaw"}; + command_axis_names_ = {"command_x", "command_y", "command_z", + "command_roll", "command_pitch", "command_yaw"}; + command_interface_types_ = {"position", "velocity"}; + state_interface_types_ = {}; + axis_pos_.resize(axis_names_.size(), 0.0); + axis_state_pos_.resize(axis_names_.size(), 0.0); + axis_vel_.resize(axis_names_.size(), 0.0); + axis_state_vel_.resize(axis_names_.size(), 0.0); + axis_acc_.resize(axis_names_.size(), 0.0); + axis_state_acc_.resize(axis_names_.size(), 0.0); + axis_eff_.resize(axis_names_.size(), 0.0); + std::size_t const num_axes = 6; + + // set up controller into open loop mode + rclcpp::executors::SingleThreadedExecutor executor; + + std::vector params = { + {"open_loop_control", true}, + {"use_feedback", true}, + {"allow_integration_in_goal_trajectories", true}, + {"hold_last_velocity", true}, + {"joint_limiter_type", "joint_limits/JointSaturationLimiter"}, + // joint limits for x + {"joint_limits.x.has_position_limits", false}, + {"joint_limits.x.has_velocity_limits", true}, + {"joint_limits.x.max_velocity", 1.0}, + {"joint_limits.x.has_acceleration_limits", true}, + {"joint_limits.x.max_acceleration", 1.0}, + {"joint_limits.x.has_deceleration_limits", true}, + {"joint_limits.x.max_deceleration", 1.0}, + // joint limits for y + {"joint_limits.y.has_position_limits", false}, + {"joint_limits.y.has_velocity_limits", true}, + {"joint_limits.y.max_velocity", 2.0}, + {"joint_limits.y.has_acceleration_limits", true}, + {"joint_limits.y.max_acceleration", 2.0}, + {"joint_limits.y.has_deceleration_limits", true}, + {"joint_limits.y.max_deceleration", 1.0}, + }; + std::vector vel_limits = {1.0, 2.0, 1.0, + 1.0, 1.0, 1.0}; // these should match limits above + std::vector acc_limits = {1.0, 2.0, 1.0, + 1.0, 1.0, 1.0}; // these should match limits above + + SetUpTrajectoryController(executor, params); + traj_controller_->get_node()->configure(); + for (std::size_t i = 0; i < 2; ++i) + { + publish_feedback({0, 0, 0}, {1, 0, 0, 0}, {0, 0, 0}, {0, 0, 0}); + executor.spin_some(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // set all initial states to 6-vector of zeros + std::vector zeros(6, 0); + ActivateTrajectoryController(true, zeros, zeros, zeros, zeros); + + // [axis-mult] At 20 Hz, sends a 'reference' command with all zeros and time from start of 50ms + // (i.e. positions are NaN, velocities are zero and accelerations are NaN) + + constexpr std::size_t freq_Hz = 20; + std::size_t const ns_dur = 1000000000 / freq_Hz; + auto const chrono_duration = std::chrono::nanoseconds(ns_dur); + rclcpp::Duration const dur(0, ns_dur); + double nan = std::numeric_limits::quiet_NaN(); + std::vector point_nan(num_axes, nan); + std::vector point_zero(num_axes, 0); + + // start with zero vels and nan positions for 1 second + std::vector> positions(freq_Hz, point_nan); + std::vector> velocities(freq_Hz, point_zero); + + std::vector expected_actual; + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + std::vector expected_desired; + expected_desired.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + expected_actual[i].position = 0; + expected_actual[i].velocity = 0; + } + + expected_desired = expected_actual; + + publish(dur, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + traj_controller_->wait_for_trajectory(executor); + + // now test that we haven't moved + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); + + // send 'reference' command with single velocity point that exceed acceleration limits and verify + // output is limited send velocity of 1.0 for x and 2.0 for y over 1 second + + std::vector desired_vel = {2.0, 4.0, 0, + 0, 0, 0}; // desired velocity for x and y exceed limits + double duration_s = 1.0; + rclcpp::Duration const dur_joint_limit = rclcpp::Duration::from_seconds(duration_s); + + positions = {1, point_nan}; + velocities.push_back(desired_vel); + + expected_actual.resize(num_axes, multi_time_trajectory_controller::emptyTrajectoryPoint()); + + for (std::size_t i = 0; i < num_axes; ++i) + { + if ((i == 0) || (i == 1)) + { + auto desired_acc = desired_vel[i] / duration_s; + + // First calculate, final position and velocity for the constant acceleration portion + // for only constant acceleration i.e. p = 0.5 * a * t^2 where t = duration_s + // the final velocity should be for constant acceleration i.e. v1 = a * t where t = duration_s + expected_actual[i].position = 0.5 * acc_limits[i] * duration_s * duration_s; + expected_actual[i].velocity = acc_limits[i] * duration_s; + + // If the desired acceleration is less than the acceleration limits, then add constant + // velocity portion + if (desired_acc < acc_limits[i]) + { + auto accel_time = desired_vel[i] / acc_limits[i]; + expected_actual[i].position += desired_vel[i] * (duration_s - accel_time); + expected_actual[i].velocity = desired_vel[i]; + } + } + else + { + expected_actual[i].position = 0; + expected_actual[i].velocity = 0; + } + } + + expected_desired = expected_actual; + + publish(dur_joint_limit, positions, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, velocities); + traj_controller_->wait_for_trajectory(executor); + + waitAndCompareState( + expected_actual, expected_desired, executor, chrono_duration * freq_Hz, 0.1, + rclcpp::Time(0, 0, RCL_STEADY_TIME), true); + positions.clear(); + velocities.clear(); + expected_actual.clear(); + expected_desired.clear(); +} diff --git a/multi_time_trajectory_controller/test/test_mttc_utils.hpp b/multi_time_trajectory_controller/test/test_mttc_utils.hpp new file mode 100644 index 0000000000..2e6601bf4e --- /dev/null +++ b/multi_time_trajectory_controller/test/test_mttc_utils.hpp @@ -0,0 +1,954 @@ +// 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. + +#ifndef TEST_MTTC_UTILS_HPP_ +#define TEST_MTTC_UTILS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "control_msgs/msg/axis_trajectory_point.hpp" +#include "control_msgs/msg/multi_axis_trajectory.hpp" +#include "control_msgs/msg/multi_time_trajectory_controller_state.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "multi_time_trajectory_controller/multi_time_trajectory_controller.hpp" + +namespace +{ +std::size_t const DOF = 3; +const double COMMON_THRESHOLD = 0.001; +const double INITIAL_POS_AXIS1 = 1.1; +const double INITIAL_POS_AXIS2 = 2.1; +const double INITIAL_POS_AXIS3 = 3.1; +const std::vector INITIAL_POS_AXES = { + INITIAL_POS_AXIS1, INITIAL_POS_AXIS2, INITIAL_POS_AXIS3}; +const std::vector INITIAL_VEL_AXES = {0.0, 0.0, 0.0}; +const std::vector INITIAL_ACC_AXES = {0.0, 0.0, 0.0}; +const std::vector INITIAL_EFF_AXES = {0.0, 0.0, 0.0}; + +bool is_same_sign_or_zero(double val1, double val2) +{ + return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); +} + +} // namespace + +namespace test_mttc +{ +class TestableMultiTimeTrajectoryController +: public multi_time_trajectory_controller::MultiTimeTrajectoryController +{ +private: +public: + using multi_time_trajectory_controller::MultiTimeTrajectoryController:: + MultiTimeTrajectoryController; + using multi_time_trajectory_controller::MultiTimeTrajectoryController::validate_trajectory_msg; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + multi_time_trajectory_controller::MultiTimeTrajectoryController::on_configure(previous_state); + return ret; + } + + rclcpp::NodeOptions define_custom_node_options() const override { return node_options_; } + + /** + * @brief wait_for_trajectory block until a new JointTrajectory is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_trajectory( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{10}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void set_axis_names(const std::vector & axis_names) { params_.axes = axis_names; } + + void set_command_axis_names(const std::vector & command_axis_names) + { + command_axis_names_ = command_axis_names; + } + + void set_command_interfaces(const std::vector & command_interfaces) + { + params_.command_interfaces = command_interfaces; + } + + void set_state_interfaces(const std::vector & state_interfaces) + { + params_.state_interfaces = state_interfaces; + } + + void trigger_declare_parameters() { param_listener_->declare_params(); } + + void testable_compute_error( + std::vector & error, + const std::vector & current, + const std::vector & desired) + { + compute_error(error, current, desired); + } + + std::vector get_current_state_when_offset() const + { + return last_commanded_state_; + } + bool has_position_state_interface() const { return has_position_state_interface_; } + + bool has_velocity_state_interface() const { return has_velocity_state_interface_; } + + bool has_acceleration_state_interface() const { return has_acceleration_state_interface_; } + + bool has_position_command_interface() const { return has_position_command_interface_; } + + bool has_velocity_command_interface() const { return has_velocity_command_interface_; } + + bool has_acceleration_command_interface() const { return has_acceleration_command_interface_; } + + bool has_effort_command_interface() const { return has_effort_command_interface_; } + + bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } + + bool is_open_loop() const { return params_.open_loop_control; } + + std::vector get_pids() const { return pids_; } + + multi_time_trajectory_controller::SegmentTolerances get_tolerances() const + { + return default_tolerances_; + } + + bool has_active_traj() const { return has_active_trajectory(); } + + bool has_trivial_traj(std::size_t axis) const + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(axis) == false; + } + + bool has_nontrivial_traj(std::size_t axis) + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(axis); + } + + double get_cmd_timeout() { return cmd_timeout_; } + + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + + std::vector get_state_feedback() + { + return state_current_; + } + std::vector get_state_reference() + { + return state_desired_; + } + std::vector get_state_error() { return state_error_; } + + rclcpp::NodeOptions node_options_; +}; + +class TrajectoryControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + virtual void SetUp() + { + controller_name_ = "test_multi_axis_controller"; + + axis_names_ = {"axis1", "axis2", "axis3"}; + command_axis_names_ = { + "following_controller/axis1", "following_controller/axis2", "following_controller/axis3"}; + axis_pos_.resize(axis_names_.size(), 0.0); + axis_state_pos_.resize(axis_names_.size(), 0.0); + axis_vel_.resize(axis_names_.size(), 0.0); + axis_state_vel_.resize(axis_names_.size(), 0.0); + axis_acc_.resize(axis_names_.size(), 0.0); + axis_state_acc_.resize(axis_names_.size(), 0.0); + axis_eff_.resize(axis_names_.size(), 0.0); + // Default interface values - they will be overwritten by parameterized tests + command_interface_types_ = {"position"}; + state_interface_types_ = {"position", "velocity"}; + + node_ = std::make_shared("trajectory_publisher_"); + trajectory_publisher_ = node_->create_publisher( + controller_name_ + "/axis_trajectory", rclcpp::SystemDefaultsQoS()); + feedback_publisher_ = node_->create_publisher< + multi_time_trajectory_controller::MultiTimeTrajectoryController::ControllerFeedbackMsg>( + controller_name_ + "/feedback", rclcpp::SystemDefaultsQoS()); + + trajectory_generator_publisher_ = + node_->create_publisher( + controller_name_ + "/reference", rclcpp::QoS(1).best_effort()); + trajectory_generator_publisher_reliable_ = + node_->create_publisher( + controller_name_ + "/reference_reliable", rclcpp::QoS(1).reliable()); + create_reset_dofs_service_client(); + } + + virtual void TearDown() + { + shutdown_ = true; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + traj_gen_sync_thread_.join(); + } + + void SetUpTrajectoryController( + rclcpp::Executor & executor, const std::vector & parameters = {}, + const std::string & urdf = "") + { + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; }) != + parameters.end(); + std::vector parameters_local = parameters; + auto has_hold_vel_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "hold_last_velocity"; }) != parameters.end(); + auto has_use_feedback_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "use_feedback"; }) != parameters.end(); + if (!has_hold_vel_param) + { + // The MAC's default behavior differs from the JTC in terms of holding the final velocity + // instead of position This parameter allows disabling that so the JTC tests can be reused + parameters_local.emplace_back("hold_last_velocity", false); + } + if (!has_use_feedback_param) + { + // The MAC can use state interfaces or feedback from a topic + parameters_local.emplace_back("use_feedback", false); + } + if (!has_nonzero_vel_param) + { + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); + } + auto ret = SetUpTrajectoryControllerLocal(parameters_local, urdf); + if (ret != controller_interface::return_type::OK) + { + FAIL(); + } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + } + + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}, const std::string & urdf = "") + { + traj_controller_ = std::make_shared(); + + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + parameter_overrides.push_back(rclcpp::Parameter("axes", axis_names_)); + parameter_overrides.push_back( + rclcpp::Parameter("command_interfaces", command_interface_types_)); + parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + traj_controller_->set_node_options(node_options); + + return traj_controller_->init( + controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); + } + + void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) + { + traj_controller_->trigger_declare_parameters(); + auto node = traj_controller_->get_node(); + + for (size_t i = 0; i < axis_names_.size(); ++i) + { + const std::string prefix = "gains." + axis_names_[i]; + const rclcpp::Parameter k_p(prefix + ".p", p_value); + const rclcpp::Parameter k_i(prefix + ".i", 0.0); + const rclcpp::Parameter k_d(prefix + ".d", 0.0); + const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + } + } + + void SetUpAndActivateTrajectoryController( + rclcpp::Executor & executor, const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + const std::vector & initial_pos_axes = INITIAL_POS_AXES, + const std::vector & initial_vel_axes = INITIAL_VEL_AXES, + const std::vector & initial_acc_axes = INITIAL_ACC_AXES, + const std::vector & initial_eff_axes = INITIAL_EFF_AXES, const std::string & urdf = "") + { + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; }) != + parameters.end(); + + std::vector parameters_local = parameters; + auto has_hold_vel_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "hold_last_velocity"; }) != parameters.end(); + + auto has_use_feedback_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "use_feedback"; }) != parameters.end(); + if (!has_hold_vel_param) + { + // The MAC's default behavior differs from the JTC in terms of holding the final velocity + // instead of position This parameter allows disabling that so the JTC tests can be reused + parameters_local.emplace_back("hold_last_velocity", false); + } + if (!has_use_feedback_param) + { + // The MAC can use state interfaces or feedback from a topic + parameters_local.emplace_back("use_feedback", false); + } + + if (!has_nonzero_vel_param) + { + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); + } + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local, urdf); + + // set pid parameters before configure + SetPidParameters(k_p, ff); + traj_controller_->get_node()->configure(); + + ActivateTrajectoryController( + separate_cmd_and_state_values, initial_pos_axes, initial_vel_axes, initial_acc_axes, + initial_eff_axes); + } + + rclcpp_lifecycle::State ActivateTrajectoryController( + bool separate_cmd_and_state_values = false, + const std::vector & initial_pos_axes = INITIAL_POS_AXES, + const std::vector & initial_vel_axes = INITIAL_VEL_AXES, + const std::vector & initial_acc_axes = INITIAL_ACC_AXES, + const std::vector & initial_eff_axes = INITIAL_EFF_AXES) + { + std::vector cmd_interfaces; + std::vector state_interfaces; + pos_cmd_interfaces_.reserve(axis_names_.size()); + vel_cmd_interfaces_.reserve(axis_names_.size()); + acc_cmd_interfaces_.reserve(axis_names_.size()); + eff_cmd_interfaces_.reserve(axis_names_.size()); + pos_state_interfaces_.reserve(axis_names_.size()); + vel_state_interfaces_.reserve(axis_names_.size()); + acc_state_interfaces_.reserve(axis_names_.size()); + for (size_t i = 0; i < axis_names_.size(); ++i) + { + pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + axis_names_[i], hardware_interface::HW_IF_POSITION, &axis_pos_[i])); + vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + axis_names_[i], hardware_interface::HW_IF_VELOCITY, &axis_vel_[i])); + acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + axis_names_[i], hardware_interface::HW_IF_ACCELERATION, &axis_acc_[i])); + eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + axis_names_[i], hardware_interface::HW_IF_EFFORT, &axis_eff_[i])); + + pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( + axis_names_[i], hardware_interface::HW_IF_POSITION, + separate_cmd_and_state_values ? &axis_state_pos_[i] : &axis_pos_[i])); + vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( + axis_names_[i], hardware_interface::HW_IF_VELOCITY, + separate_cmd_and_state_values ? &axis_state_vel_[i] : &axis_vel_[i])); + acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( + axis_names_[i], hardware_interface::HW_IF_ACCELERATION, + separate_cmd_and_state_values ? &axis_state_acc_[i] : &axis_acc_[i])); + + // Add to export lists and set initial values + cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); + if (!cmd_interfaces.back().set_value(initial_pos_axes[i])) + { + throw std::runtime_error("Failed to set initial position value for axis " + axis_names_[i]); + } + cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); + if (!cmd_interfaces.back().set_value(initial_vel_axes[i])) + { + throw std::runtime_error("Failed to set initial velocity value for axis " + axis_names_[i]); + } + cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); + if (!cmd_interfaces.back().set_value(initial_acc_axes[i])) + { + throw std::runtime_error( + "Failed to set initial acceleration value for axis " + axis_names_[i]); + } + cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); + if (!cmd_interfaces.back().set_value(initial_eff_axes[i])) + { + throw std::runtime_error("Failed to set initial effort value for axis " + axis_names_[i]); + } + if (separate_cmd_and_state_values) + { + axis_state_pos_[i] = INITIAL_POS_AXES[i]; + axis_state_vel_[i] = INITIAL_VEL_AXES[i]; + axis_state_acc_[i] = INITIAL_ACC_AXES[i]; + } + state_interfaces.emplace_back(pos_state_interfaces_.back()); + state_interfaces.emplace_back(vel_state_interfaces_.back()); + state_interfaces.emplace_back(acc_state_interfaces_.back()); + } + + traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); + return traj_controller_->get_node()->activate(); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void subscribeToState(rclcpp::Executor & executor) + { + auto traj_lifecycle_node = traj_controller_->get_node(); + + using control_msgs::msg::MultiTimeTrajectoryControllerState; + + auto qos = rclcpp::SensorDataQoS(); + // Needed, otherwise spin_some() returns only the oldest message in the queue + // I do not understand why spin_some provides only one message + qos.keep_last(1); + state_subscriber_ = + traj_lifecycle_node->create_subscription( + controller_name_ + "/controller_state", qos, + [&](std::shared_ptr msg) + { + std::lock_guard guard(state_mutex_); + state_msg_ = msg; + }); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = traj_lifecycle_node->get_clock()->now() + timeout; + while (traj_lifecycle_node->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + /// Publish trajectory msgs with multiple points + /** + * delay_btwn_points - delay between each point + * points_positions - vector of trajectory-positions. One point per controlled joint + * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided + * points + * points - vector of trajectory-velocities. One point per controlled joint + */ + void publish( + const builtin_interfaces::msg::Duration & delay_btwn_points, + const std::vector> & points_positions, rclcpp::Time start_time, + const std::vector & axis_names = {}, + const std::vector> & points_velocities = {}, bool use_traj_gen = false, + bool reliable = false) + { + int wait_count = 0; + const auto topic = trajectory_publisher_->get_topic_name(); + while (node_->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + control_msgs::msg::MultiAxisTrajectory multi_traj_msg; + std::size_t num_axes = points_positions.at(0).size(); + multi_traj_msg.axis_trajectories.resize(num_axes); + if (axis_names.empty()) + { + multi_traj_msg.axis_names = { + axis_names_.begin(), axis_names_.begin() + static_cast(num_axes)}; + } + else + { + multi_traj_msg.axis_names = axis_names; + } + multi_traj_msg.header.stamp = start_time; + + for (std::size_t i = 0; i < num_axes; ++i) + { + auto & traj_msg = multi_traj_msg.axis_trajectories.at(i); + + traj_msg.axis_points.resize( + points_positions.size(), multi_time_trajectory_controller::emptyTrajectoryPoint()); + + builtin_interfaces::msg::Duration duration_msg; + duration_msg.sec = delay_btwn_points.sec; + duration_msg.nanosec = delay_btwn_points.nanosec; + rclcpp::Duration duration(duration_msg); + rclcpp::Duration duration_total(duration_msg); + + for (size_t index = 0; index < points_positions.size(); ++index) + { + traj_msg.axis_points.at(index).time_from_start = duration_total; + + traj_msg.axis_points.at(index).position = points_positions.at(index).at(i); + traj_msg.axis_points.at(index).time_from_start = duration_total; + duration_total += duration; + } + + for (size_t index = 0; index < points_velocities.size(); ++index) + { + traj_msg.axis_points.at(index).velocity = points_velocities.at(index).at(i); + } + } + + if (use_traj_gen) + { + if (reliable) + { + trajectory_generator_publisher_reliable_->publish(multi_traj_msg); + } + else + { + trajectory_generator_publisher_->publish(multi_traj_msg); + } + } + else + { + trajectory_publisher_->publish(multi_traj_msg); + } + } + + void publish_feedback( + Eigen::Vector3d position, Eigen::Quaterniond orientation, Eigen::Vector3d v_linear, + Eigen::Vector3d v_angular) + { + multi_time_trajectory_controller::MultiTimeTrajectoryController::ControllerFeedbackMsg message; + message.pose.pose.position.x = position[0]; + message.pose.pose.position.y = position[1]; + message.pose.pose.position.z = position[2]; + + message.pose.pose.orientation.w = orientation.w(); + message.pose.pose.orientation.x = orientation.x(); + message.pose.pose.orientation.y = orientation.y(); + message.pose.pose.orientation.z = orientation.z(); + + message.twist.twist.linear.x = v_linear[0]; + message.twist.twist.linear.y = v_linear[1]; + message.twist.twist.linear.z = v_linear[2]; + + message.twist.twist.angular.x = v_angular[0]; + message.twist.twist.angular.y = v_angular[1]; + message.twist.twist.angular.z = v_angular[2]; + + feedback_publisher_->publish(message); + } + + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const auto end_time = start_time + wait_time; + auto previous_time = start_time; + + while (clock.now() <= end_time) + { + auto now = clock.now(); + traj_controller_->update(now, now - previous_time); + previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); + } + } + + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01), + rclcpp::Executor * executor = nullptr) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); + } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + if (executor != nullptr) + { + // publish commanded state as feedback (assume perfect control) + Eigen::Vector3d position = {axis_pos_[0], axis_pos_[1], axis_pos_[2]}; + double roll = axis_pos_[3]; + double pitch = axis_pos_[4]; + double yaw = axis_pos_[5]; + Eigen::Quaterniond orientation = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + + Eigen::Vector3d v_linear = {axis_vel_[0], axis_vel_[1], axis_vel_[2]}; + Eigen::Vector3d v_angular = {axis_vel_[3], axis_vel_[4], axis_vel_[5]}; + + publish_feedback(position, orientation, v_linear, v_angular); + executor->spin_some(); + } + } + return end_time; + } + + rclcpp::Time waitAndCompareState( + std::vector const & expected_actual, + std::vector const & expected_desired, + rclcpp::Executor & executor, rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + bool should_publish_feedback = false) + { + { + std::lock_guard guard(state_mutex_); + state_msg_.reset(); + } + traj_controller_->wait_for_trajectory(executor); + rclcpp::Time end_time; + if (should_publish_feedback) + { + end_time = updateControllerAsync( + controller_wait_time, start_time, rclcpp::Duration::from_seconds(0.01), &executor); + } + else + { + end_time = updateControllerAsync(controller_wait_time, start_time); + } + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + + for (size_t i = 0; i < expected_actual.size(); ++i) + { + SCOPED_TRACE("Axis " + std::to_string(i)); + // TODO(anyone): add checking for velocities and accelerations + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_actual[i].position, state_feedback[i].position, allowed_delta); + } + } + + for (size_t i = 0; i < expected_desired.size(); ++i) + { + SCOPED_TRACE("Axis " + std::to_string(i)); + // TODO(anyone): add checking for velocities and accelerations + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_desired[i].position, state_reference[i].position, allowed_delta); + } + } + + return end_time; + } + + std::shared_ptr getState() const + { + std::lock_guard guard(state_mutex_); + return state_msg_; + } + + void expectCommandPoint( + std::vector const & position, std::vector const & velocity = {0.0, 0.0, 0.0}) + { + // it should be holding the given point + // i.e., active but trivial trajectory (one point only) + for (std::size_t i = 0; i < 3; ++i) + { + EXPECT_TRUE(traj_controller_->has_trivial_traj(i)); + } + + if (traj_controller_->use_closed_loop_pid_adapter() == false) + { + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(position.at(0), axis_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), axis_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), axis_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(velocity.at(0), axis_vel_[0]); + EXPECT_EQ(velocity.at(1), axis_vel_[1]); + EXPECT_EQ(velocity.at(2), axis_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, axis_acc_[0]); + EXPECT_EQ(0.0, axis_acc_[1]); + EXPECT_EQ(0.0, axis_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, axis_eff_[0]); + EXPECT_EQ(0.0, axis_eff_[1]); + EXPECT_EQ(0.0, axis_eff_[2]); + } + } + else // traj_controller_->use_closed_loop_pid_adapter() == true + { + // velocity or effort PID? + // --> set kp > 0.0 in test + if (traj_controller_->has_velocity_command_interface()) + { + for (size_t i = 0; i < 3; i++) + { + double val; + EXPECT_TRUE( + pos_state_interfaces_[i].get_value(val) && + is_same_sign_or_zero(position.at(i) - val, axis_vel_[i])) + << "test position point " << position.at(i) << ", position state is " << val + << ", velocity command is " << axis_vel_[i]; + } + } + if (traj_controller_->has_effort_command_interface()) + { + for (size_t i = 0; i < 3; i++) + { + double val; + EXPECT_TRUE( + pos_state_interfaces_[i].get_value(val) && + is_same_sign_or_zero(position.at(i) - val, axis_eff_[i])) + << "test position point " << position.at(i) << ", position state is " << val + << ", effort command is " << axis_eff_[i]; + } + } + } + } + + void expectHoldingPointDeactivated(std::vector const & point) + { + // it should be holding the given point, but no active trajectory + EXPECT_FALSE(traj_controller_->has_active_traj()); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), axis_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), axis_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), axis_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, axis_vel_[0]); + EXPECT_EQ(0.0, axis_vel_[1]); + EXPECT_EQ(0.0, axis_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, axis_acc_[0]); + EXPECT_EQ(0.0, axis_acc_[1]); + EXPECT_EQ(0.0, axis_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, axis_eff_[0]); + EXPECT_EQ(0.0, axis_eff_[1]); + EXPECT_EQ(0.0, axis_eff_[2]); + } + } + + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_axes( + std::vector const & state_axis_names, + std::vector const & command_axis_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_axis_names.size() * state_interface_types_.size()); + for (const auto & axis : state_axis_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(axis + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_axis_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_axis_names.size() * command_interface_types_.size()); + for (const auto & axis : command_axis_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(axis + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), command_axis_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + + std::string controller_name_; + + std::vector axis_names_; + std::vector command_axis_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr trajectory_publisher_; + rclcpp::Publisher::SharedPtr feedback_publisher_; + + bool shutdown_{false}; + bool traj_gen_available_{false}; + std::thread traj_gen_sync_thread_; + rclcpp::Time traj_gen_sync_start_time_; + rclcpp::Client::SharedPtr traj_gen_reset_dofs_client_; + rclcpp::Publisher::SharedPtr + trajectory_generator_publisher_; + rclcpp::Publisher::SharedPtr + trajectory_generator_publisher_reliable_; + + struct ResetDofsData + { + std::size_t motion_axis; + bool closed_loop_position_enabled; + }; + + bool send_reset_request( + std::shared_ptr request, rclcpp::Executor & executor) + { + if (!traj_gen_available_) + { + throw std::runtime_error("Reset dofs service not yet available."); + } + auto result = traj_gen_reset_dofs_client_->async_send_request(request); + auto retval = executor.spin_until_future_complete(result, std::chrono::seconds(1)); + + return retval == rclcpp::FutureReturnCode::SUCCESS; + } + void create_reset_dofs_service_client() + { + traj_gen_sync_thread_ = std::thread( + [this]() + { + const auto clk = node_->get_clock(); + traj_gen_sync_start_time_ = clk->now(); + + // Create a service client for resetting dofs in trajectory generator + const std::string service_name = controller_name_ + "/reset_dofs"; + traj_gen_reset_dofs_client_ = + node_->create_client(service_name); + while (!traj_gen_reset_dofs_client_->wait_for_service(std::chrono::milliseconds(10))) + { + if (shutdown_) break; + } + + if (!shutdown_) + { + traj_gen_available_ = true; + } + }); + } + std::shared_ptr traj_controller_; + rclcpp::Subscription::SharedPtr + state_subscriber_; + mutable std::mutex state_mutex_; + std::shared_ptr state_msg_; + + std::vector axis_state; + std::vector axis_pos_; + std::vector axis_vel_; + std::vector axis_acc_; + std::vector axis_eff_; + std::vector axis_state_pos_; + std::vector axis_state_vel_; + std::vector axis_state_acc_; + std::vector pos_cmd_interfaces_; + std::vector vel_cmd_interfaces_; + std::vector acc_cmd_interfaces_; + std::vector eff_cmd_interfaces_; + std::vector pos_state_interfaces_; + std::vector vel_state_interfaces_; + std::vector acc_state_interfaces_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class TrajectoryControllerTestParameterized +: public TrajectoryControllerTest, + public ::testing::WithParamInterface< + std::tuple, std::vector>> +{ +public: + virtual void SetUp() + { + TrajectoryControllerTest::SetUp(); + command_interface_types_ = std::get<0>(GetParam()); + state_interface_types_ = std::get<1>(GetParam()); + } + + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } +}; + +} // namespace test_mttc + +#endif // TEST_MTTC_UTILS_HPP_ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 4495828bff..ab9b7cdca4 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -30,6 +30,7 @@ joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller + multi_time_trajectory_controller parallel_gripper_controller pid_controller pose_broadcaster