diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 62b2e8f8..fe9fc48a 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -43,7 +43,6 @@ RUN apt-get update && \ ros-humble-joint-state-broadcaster \ ros-humble-ament-cmake-clang-tidy \ libignition-gazebo6-dev \ - ros-humble-joint-trajectory-controller \ libpoco-dev \ libeigen3-dev \ && rm -rf /var/lib/apt/lists/* diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 15e5a9ff..7394af76 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -13,15 +13,7 @@ "bungcip.better-toml", "ms-vscode.cpptools", "ms-vscode.cpptools-extension-pack" - ], - "settings": { - "terminal.integrated.defaultProfile.linux": "bash", - "terminal.integrated.profiles.linux": { - "bash": { - "path": "/bin/bash" - } - } - } + ] } } } \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index 580027ac..72d90464 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,10 +1,17 @@ # Changelog +## 0.1.8 - 2023-11-16 + +Requires libfranka >= 0.13.0, required ROS 2 Humble + +* franka\_hardware: add unit tests for robot class. +* joint\_trajectory\_controller: hotfix add joint patched old JTC back. + ## 0.1.7 - 2023-11-10 Requires libfranka >= 0.12.1, required ROS 2 Humble -* franka\_hardware: joint position command inteface supported +* franka\_hardware: joint position command interface supported * franka\_hardware: controller initializer automatically acknowledges error, if arm is in reflex mode * franka\_example\_controllers: joint position example controller provided * franka\_example\_controllers: fix second start bug with the example controllers diff --git a/Dockerfile b/Dockerfile index 8c2233e0..eaa69cc7 100644 --- a/Dockerfile +++ b/Dockerfile @@ -59,7 +59,7 @@ RUN python3 -m pip install -U \ RUN mkdir ~/source_code RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \ && cd libfranka \ - && git checkout 0.12.1 \ + && git checkout 0.13.0 \ && git submodule init \ && git submodule update \ && mkdir build && cd build \ diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index 01e80aa7..9a44b6f8 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.7 + 0.1.8 Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_description/package.xml b/franka_description/package.xml index 4d5e54fb..72ab1101 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.7 + 0.1.8 franka_description contains URDF files and meshes of Franka Emika robots Franka Emika GmbH Apache 2.0 diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index 19f8f0fc..18afb684 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.7 + 0.1.8 franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_gripper/CMakeLists.txt b/franka_gripper/CMakeLists.txt index b386361c..3d4b74e0 100644 --- a/franka_gripper/CMakeLists.txt +++ b/franka_gripper/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(std_srvs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(control_msgs REQUIRED) -find_package(Franka REQUIRED) +find_package(Franka 0.13.0 REQUIRED) add_library(gripper_server SHARED src/gripper_action_server.cpp) diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index de84f1bc..63fe81cc 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.7 + 0.1.8 This package implements the franka gripper of type Franka Hand for the use in ROS2 Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/CMakeLists.txt b/franka_hardware/CMakeLists.txt index 12a4fa1b..49c9b01a 100644 --- a/franka_hardware/CMakeLists.txt +++ b/franka_hardware/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(rclcpp REQUIRED) find_package(franka_msgs REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) -find_package(Franka 0.12.1 REQUIRED) +find_package(Franka 0.13.0 REQUIRED) find_package(rclcpp_components REQUIRED) add_library(franka_hardware diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index fd3d67e7..2952651e 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -44,6 +45,13 @@ namespace franka_hardware { class Robot { public: + /** + * @brief Construct a new Robot object for tests purposes + * + * @param robot mocked libfranka robot + * @param model mocked model object + */ + explicit Robot(std::unique_ptr robot, std::unique_ptr model); /** * Connects to the robot. This method can block for up to one minute if the robot is not * responding. An exception will be thrown if the connection cannot be established. @@ -262,17 +270,16 @@ class Robot { std::mutex control_mutex_; std::unique_ptr robot_; - std::unique_ptr active_control_; + std::unique_ptr active_control_ = nullptr; std::unique_ptr model_; std::unique_ptr franka_hardware_model_; - std::array last_desired_torque_ = {0, 0, 0, 0, 0, 0, 0}; - bool effort_interface_active_{false}; bool joint_velocity_interface_active_{false}; bool joint_position_interface_active_{false}; bool cartesian_velocity_interface_active_{false}; + bool torque_command_rate_limiter_active_{true}; bool velocity_command_rate_limit_active_{false}; bool cartesian_velocity_command_rate_limit_active_{false}; diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index ab0a9c59..ac72c788 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.7 + 0.1.8 franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index 5b2ca57b..44253933 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -24,6 +24,9 @@ namespace franka_hardware { +Robot::Robot(std::unique_ptr robot, std::unique_ptr model) + : robot_(std::move(robot)), franka_hardware_model_(std::move(model)) {} + Robot::Robot(const std::string& robot_ip, const rclcpp::Logger& logger) { franka::RealtimeConfig rt_config = franka::RealtimeConfig::kEnforce; if (!franka::hasRealtimeKernel()) { @@ -65,6 +68,9 @@ void Robot::stopRobot() { } void Robot::writeOnce(const std::array& joint_commands) { + if (!active_control_) { + throw std::runtime_error("Control hasn't been started"); + } if (effort_interface_active_) { writeOnceEfforts(joint_commands); } else if (joint_velocity_interface_active_) { @@ -78,10 +84,10 @@ void Robot::writeOnceEfforts(const std::array& efforts) { std::lock_guard lock(control_mutex_); auto torque_command = franka::Torques(efforts); - torque_command.tau_J = - franka::limitRate(franka::kMaxTorqueRate, torque_command.tau_J, last_desired_torque_); - last_desired_torque_ = torque_command.tau_J; - + if (torque_command_rate_limiter_active_) { + torque_command.tau_J = + franka::limitRate(franka::kMaxTorqueRate, torque_command.tau_J, current_state_.tau_J_d); + } active_control_->writeOnce(torque_command); } @@ -162,6 +168,10 @@ void Robot::preProcessCartesianVelocities(franka::CartesianVelocities& velocity_ } void Robot::writeOnce(const std::array& cartesian_velocities) { + if (!active_control_) { + throw std::runtime_error("Control hasn't been started"); + } + std::lock_guard lock(control_mutex_); auto velocity_command = franka::CartesianVelocities(cartesian_velocities); @@ -172,6 +182,10 @@ void Robot::writeOnce(const std::array& cartesian_velocities) { void Robot::writeOnce(const std::array& cartesian_velocities, const std::array& elbow_command) { + if (!active_control_) { + throw std::runtime_error("Control hasn't been started"); + } + std::lock_guard lock(control_mutex_); auto velocity_command = franka::CartesianVelocities(cartesian_velocities, elbow_command); diff --git a/franka_hardware/test/CMakeLists.txt b/franka_hardware/test/CMakeLists.txt index fe5ee664..f7f7a334 100644 --- a/franka_hardware/test/CMakeLists.txt +++ b/franka_hardware/test/CMakeLists.txt @@ -10,3 +10,8 @@ ament_add_gmock(${PROJECT_NAME}_command_interface_test franka_hardware_cartesian target_include_directories(${PROJECT_NAME}_command_interface_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) target_link_libraries(${PROJECT_NAME}_command_interface_test ${PROJECT_NAME}) + +ament_add_gmock(${PROJECT_NAME}_robot_test franka_robot_test.cpp) +target_include_directories(${PROJECT_NAME}_robot_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) + +target_link_libraries(${PROJECT_NAME}_robot_test ${PROJECT_NAME}) diff --git a/franka_hardware/test/franka_robot_test.cpp b/franka_hardware/test/franka_robot_test.cpp new file mode 100644 index 00000000..ee11abf1 --- /dev/null +++ b/franka_hardware/test/franka_robot_test.cpp @@ -0,0 +1,149 @@ +#include "franka_robot_test.hpp" + +TEST_F(FrankaRobotTests, whenInitializeTorqueInterfaceCalled_thenStartTorqueControlCalled) { + EXPECT_CALL(*mock_libfranka_robot, startTorqueControl()).Times(1); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeTorqueInterface(); +} + +TEST_F(FrankaRobotTests, whenInitializeJointVelocityInterfaceCalled_thenStartJointVelocityControl) { + EXPECT_CALL(*mock_libfranka_robot, startJointVelocityControl(testing::_)).Times(1); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeJointVelocityInterface(); +} + +TEST_F(FrankaRobotTests, + whenInitializeJointtPositionInterfaceCalled_thenStartJointPositionControl) { + EXPECT_CALL(*mock_libfranka_robot, startJointPositionControl(testing::_)).Times(1); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeJointPositionInterface(); +} + +TEST_F(FrankaRobotTests, + whenInitializeCartesianVelocityInterfaceCalled_thenStartCartesianVelocityControl) { + EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_)).Times(1); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeCartesianVelocityInterface(); +} + +TEST_F(FrankaRobotTests, + givenCartesianVelocityControlIsStarted_whenReadOnceIsCalled_expectCorrectRobotState) { + franka::RobotState robot_state; + franka::Duration duration; + robot_state.q_d = std::array{1, 2, 3, 1, 2, 3, 1}; + auto active_control_read_return_tuple = std::make_pair(robot_state, duration); + + EXPECT_CALL(*mock_active_control, readOnce()) + .WillOnce(testing::Return(active_control_read_return_tuple)); + EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_)) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeCartesianVelocityInterface(); + auto actual_state = robot.readOnce(); + ASSERT_EQ(robot_state.q_d, actual_state.q_d); +} + +TEST_F(FrankaRobotTests, + givenJointControlIsNotStarted_whenReadOnceIsCalled_expectCorrectRobotState) { + franka::RobotState robot_state; + robot_state.q_d = std::array{1, 2, 3, 1, 2, 3, 1}; + + EXPECT_CALL(*mock_libfranka_robot, readOnce()).WillOnce(testing::Return(robot_state)); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + auto actual_state = robot.readOnce(); + + ASSERT_EQ(robot_state.q_d, actual_state.q_d); +} + +TEST_F( + FrankaRobotTests, + givenCartesianVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { + const std::array& cartesian_velocities{1, 0, 0, 0, 0, 0}; + const franka::CartesianVelocities expected_cartesian_velocities(cartesian_velocities); + + auto expectCallFunction = [this]() { + EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_)) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + }; + + testWriteOnce>( + &franka_hardware::Robot::initializeCartesianVelocityInterface, expectCallFunction, + cartesian_velocities, expected_cartesian_velocities); +} + +TEST_F( + FrankaRobotTests, + givenJointPositionControlIsControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { + const std::array& joint_positions{1, 0, 0, 0, 0, 0, 0}; + const franka::JointPositions expected_joint_positions(joint_positions); + + auto expectCallFunction = [this]() { + EXPECT_CALL(*mock_libfranka_robot, startJointPositionControl(testing::_)) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + }; + + testWriteOnce>( + &franka_hardware::Robot::initializeJointPositionInterface, expectCallFunction, + joint_positions, expected_joint_positions); +} + +TEST_F( + FrankaRobotTests, + givenJointVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { + const std::array& joint_velocities{1, 0, 0, 0, 0, 0, 0}; + const franka::JointVelocities expected_joint_velocities(joint_velocities); + + const auto expectCallFunction = [this]() { + EXPECT_CALL(*mock_libfranka_robot, startJointVelocityControl(testing::_)) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + }; + + testWriteOnce>( + &franka_hardware::Robot::initializeJointVelocityInterface, expectCallFunction, + joint_velocities, expected_joint_velocities); +} + +TEST_F(FrankaRobotTests, + givenEffortControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { + const std::array& joint_torques{1, 0, 0, 0, 0, 0, 0}; + // Torque rate limiter defaulted to active + const franka::Torques expected_joint_torques{0.999999, 0, 0, 0, 0, 0, 0}; + + auto expectCallFunction = [this]() { + EXPECT_CALL(*mock_libfranka_robot, startTorqueControl()) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + }; + + testWriteOnce>( + &franka_hardware::Robot::initializeTorqueInterface, expectCallFunction, joint_torques, + expected_joint_torques); +} + +TEST_F(FrankaRobotTests, + givenControlIsNotStart_whenWriteOnceIsCalled_expectRuntimeExceptionToBeThrown) { + const std::array& joint_torques{1, 0, 0, 0, 0, 0, 0}; + const franka::Torques joint_torques_franka(joint_torques); + + const std::array& cartesian_velocities{1, 0, 0, 0, 0, 0}; + const franka::CartesianVelocities cartesian_franka(cartesian_velocities); + + EXPECT_CALL(*mock_active_control, writeOnce(joint_torques_franka)).Times(0); + EXPECT_CALL(*mock_active_control, writeOnce(cartesian_franka)).Times(0); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + EXPECT_THROW(robot.writeOnce(joint_torques), std::runtime_error); + EXPECT_THROW(robot.writeOnce(cartesian_velocities), std::runtime_error); +} diff --git a/franka_hardware/test/franka_robot_test.hpp b/franka_hardware/test/franka_robot_test.hpp new file mode 100644 index 00000000..d1fe6987 --- /dev/null +++ b/franka_hardware/test/franka_robot_test.hpp @@ -0,0 +1,118 @@ +#include +#include +#include + +#include +#include +#include "franka_hardware/robot.hpp" +#include "test_utils.hpp" + +#pragma once + +const double k_EPS = 1e-5; + +class MockActiveControl : public franka::ActiveControlBase { + public: + MOCK_METHOD((std::pair), readOnce, (), (override)); + MOCK_METHOD(void, writeOnce, (const franka::Torques&), (override)); + MOCK_METHOD(void, + writeOnce, + (const franka::JointPositions&, const std::optional&), + (override)); + MOCK_METHOD(void, + writeOnce, + (const franka::JointVelocities&, const std::optional&), + (override)); + MOCK_METHOD(void, + writeOnce, + (const franka::CartesianPose&, const std::optional&), + (override)); + MOCK_METHOD(void, + writeOnce, + (const franka::CartesianVelocities&, const std::optional&), + (override)); + MOCK_METHOD(void, writeOnce, (const franka::JointPositions&), (override)); + MOCK_METHOD(void, writeOnce, (const franka::JointVelocities&), (override)); + MOCK_METHOD(void, writeOnce, (const franka::CartesianPose&), (override)); + MOCK_METHOD(void, writeOnce, (const franka::CartesianVelocities&), (override)); +}; + +class MockFrankaRobot : public franka::Robot { + public: + MOCK_METHOD(franka::RobotState, readOnce, (), (override)); + MOCK_METHOD(std::unique_ptr, startTorqueControl, (), (override)); + MOCK_METHOD(std::unique_ptr, + startJointVelocityControl, + (const research_interface::robot::Move::ControllerMode&), + (override)); + MOCK_METHOD(std::unique_ptr, + startJointPositionControl, + (const research_interface::robot::Move::ControllerMode&), + (override)); + MOCK_METHOD(std::unique_ptr, + startCartesianPoseControl, + (const research_interface::robot::Move::ControllerMode&), + (override)); + MOCK_METHOD(std::unique_ptr, + startCartesianVelocityControl, + (const research_interface::robot::Move::ControllerMode&), + (override)); +}; + +namespace franka { + +template +bool compareWithTolerance(const T& lhs, const T& rhs) { + return std::equal(lhs.begin(), lhs.end(), rhs.begin(), + [](double lhs_element, double rhs_element) { + return std::abs(lhs_element - rhs_element) < k_EPS; + }); +} + +bool operator==(const CartesianVelocities& lhs, const CartesianVelocities& rhs) { + return compareWithTolerance(lhs.O_dP_EE, rhs.O_dP_EE); +} + +bool operator==(const JointPositions& lhs, const JointPositions& rhs) { + return compareWithTolerance(lhs.q, rhs.q); +} + +bool operator==(const JointVelocities& lhs, const JointVelocities& rhs) { + return compareWithTolerance(lhs.dq, rhs.dq); +} + +bool operator==(const Torques& lhs, const Torques& rhs) { + return compareWithTolerance(lhs.tau_J, rhs.tau_J); +} +} // namespace franka + +class FrankaRobotTests : public ::testing::Test { + protected: + std::unique_ptr mock_libfranka_robot; + std::unique_ptr mock_model; + std::unique_ptr mock_active_control; + + template + void testWriteOnce(RobotInitFunction initFunction, + std::function expectCallFunction, + const RawControlInputType& control_input, + const ControlType& expected_active_control_input) { + EXPECT_CALL(*mock_active_control, writeOnce(expected_active_control_input)); + expectCallFunction(); + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + (robot.*initFunction)(); + robot.writeOnce(control_input); + } + + void SetUp() override { + mock_libfranka_robot = std::make_unique(); + mock_model = std::make_unique(); + mock_active_control = std::make_unique(); + } + void TearDown() override { + mock_libfranka_robot.reset(); + mock_model.reset(); + mock_active_control.reset(); + } +}; \ No newline at end of file diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index 80bd0957..84a581da 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.7 + 0.1.8 Contains Moveit2 configuration files for Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index d9c7b0c1..380aaad4 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.7 + 0.1.8 franka_msgs provides messages and actions specific to Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_robot_state_broadcaster/package.xml b/franka_robot_state_broadcaster/package.xml index c8dcdd25..8097c674 100644 --- a/franka_robot_state_broadcaster/package.xml +++ b/franka_robot_state_broadcaster/package.xml @@ -1,7 +1,7 @@ franka_robot_state_broadcaster - 0.1.7 + 0.1.8 Broadcaster to publish robot states Franka Emika GmbH Apache 2.0 diff --git a/franka_semantic_components/CMakeLists.txt b/franka_semantic_components/CMakeLists.txt index 6646f3a6..497981f3 100644 --- a/franka_semantic_components/CMakeLists.txt +++ b/franka_semantic_components/CMakeLists.txt @@ -20,7 +20,7 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(Franka REQUIRED) +find_package(Franka 0.13.0 REQUIRED) add_library(franka_semantic_components SHARED src/franka_robot_state.cpp diff --git a/integration_launch_testing/package.xml b/integration_launch_testing/package.xml index c31b0867..fbdab53f 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.7 + 0.1.8 Functional integration tests for franka controllers Franka Emika GmbH Apache 2.0 diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst new file mode 100644 index 00000000..5ab2f01f --- /dev/null +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -0,0 +1,286 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package joint_trajectory_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.3.0 (2023-03-07) +------------------ +* Add comments about auto-generated header files (`#539 `_) +* 🕰️ remove state publish rate from JTC. (`#520 `_) +* Contributors: AndyZe, Dr. Denis + +3.2.0 (2023-02-10) +------------------ +* fix JTC segfault (`#518 `_) +* fix interpolation logic (`#516 `_) +* Fix overriding of install (`#510 `_) +* Add JTC normalize_error parameter to doc (`#511 `_) +* Fix JTC segfault on unload (`#515 `_) +* Don't set interpolation_method\_ twice (`#517 `_) +* Remove compile warnings. (`#519 `_) +* Contributors: Andy Zelenak, Christoph Fröhlich, Dr. Denis, Michael Wiznitzer, Márk Szitanics, Solomon Wiznitzer, Tyler Weaver, Chris Thrasher + +3.1.0 (2023-01-26) +------------------ +* ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) +* [JTC] Configurable joint positon error normalization behavior (`#491 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Bence Magyar + +3.0.0 (2023-01-19) +------------------ +* [JTC] Add pid gain structure to documentation (`#485 `_) +* [JTC] Activate test for only velocity controller (`#487 `_) +* [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) +* Add backward_ros to all controllers (`#489 `_) +* Fix markup in userdoc.rst (`#480 `_) +* [JTC] Remove deprecation from parameters validation file. (`#476 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Denis Štogl + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- +* Fix parameter library export (`#448 `_) +* Contributors: Tyler Weaver + +2.13.0 (2022-10-05) +------------------- +* Generate Parameter Library for Joint Trajectory Controller (`#384 `_) +* Fix rates in JTC userdoc.rst (`#433 `_) +* Fix for high CPU usage by JTC in gzserver (`#428 `_) + * Change type cast wall timer period from second to nanoseconds. + create_wall_timer() expects delay in nanoseconds (duration object) however the type cast to seconds will result in 0 (if duration is less than 1s) and thus causing timer to be fired non stop resulting in very high CPU usage. + * Reset smartpointer so that create_wall_timer() call can destroy previous trajectory timer. + node->create_wall_timer() first removes timers associated with expired smartpointers before servicing current request. The JTC timer pointer gets overwrite only after the create_wall_timer() returns and thus not able to remove previous trajectory timer resulting in upto two timers running for JTC during trajectory execution. Althougth the previous timer does nothing but still get fired. +* Contributors: Arshad Mehmood, Borong Yuan, Tyler Weaver, Andy Zelenak, Bence Magyar, Denis Štogl + +2.12.0 (2022-09-01) +------------------- +* Use a "steady clock" when measuring time differences (`#427 `_) +* [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (`#380 `_) +* test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (`#415 `_) +* Reinstate JTC tests (`#391 `_) +* [JTC] Hold position if tolerance is violated even during non-active goal (`#368 `_) +* Small fixes for JTC. (`#390 `_) + variables in JTC to not clutter other PR with them. + fixes of updating parameters on renewed configuration of JTC that were missed +* Contributors: Andy Zelenak, Bence Magyar, Denis Štogl, Jaron Lundwall, Michael Wiznitzer + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- +* Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking +* Use system time in all tests to avoid error with different time sources. (`#334 `_) +* Contributors: Bence Magyar, Denis Štogl + +2.9.0 (2022-07-14) +------------------ +* Add option to skip interpolation in the joint trajectory controller (`#374 `_) + * Introduce `InterpolationMethods` structure + * Use parameters to define interpolation use in JTC +* Contributors: Andy Zelenak + +2.8.0 (2022-07-09) +------------------ +* Preallocate JTC variables to avoid resizing in realtime loops (`#340 `_) +* Contributors: Andy Zelenak + +2.7.0 (2022-07-03) +------------------ +* Properly retrieve parameters in the Joint Trajectory Controller (`#365 `_) +* Rename the "abort" variable in the joint traj controller (`#367 `_) +* account for edge case in JTC (`#350 `_) +* Contributors: Andy Zelenak, Michael Wiznitzer + +2.6.0 (2022-06-18) +------------------ +* Disable failing workflows (`#363 `_) +* Fixed lof message in joint_trayectory_controller (`#366 `_) +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Member variable renaming in the Joint Traj Controller (`#361 `_) +* Contributors: Alejandro Hernández Cordero, Andy Zelenak, Jafar Abdi + +2.5.0 (2022-05-13) +------------------ +* check for nans in command interface (`#346 `_) +* Contributors: Michael Wiznitzer + +2.4.0 (2022-04-29) +------------------ +* Fix a gtest deprecation warning (`#341 `_) +* Delete unused variable in joint_traj_controller (`#339 `_) +* updated to use node getter functions (`#329 `_) +* Fix JTC state tolerance and goal_time tolerance check bug (`#316 `_) + * fix state tolerance check bug + * hold position when canceling or aborting. update state tolerance test + * add goal tolerance fail test + * better state tolerance test + * use predefined constants + * fix goal_time logic and tests + * add comments +* Contributors: Andy Zelenak, Jack Center, Michael Wiznitzer, Bence Magyar, Denis Štogl + +2.3.0 (2022-04-21) +------------------ +* [JTC] Allow integration of states in goal trajectories (`#190 `_) + * Added position and velocity deduction to trajectory. + * Added support for deduction of states from their derivatives. +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* [JTC] Implement effort-only command interface (`#225 `_) + * Fix trajectory tolerance parameters + * Implement effort command interface for JTC + * Use auto_declare for pid params + * Set effort to 0 on deactivate +* [JTC] Variable renaming for clearer API (`#323 `_) +* Remove unused include to fix JTC test (`#319 `_) +* Contributors: Akash, Andy Zelenak, Bence Magyar, Denis Štogl, Jafar Abdi, Victor Lopez + +2.2.0 (2022-03-25) +------------------ +* Use lifecycle node as base for controllers (`#244 `_) +* JointTrajectoryController: added missing control_toolbox dependencies (`#315 `_) +* Use time argument on update function instead of node time (`#296 `_) +* Export dependency (`#310 `_) +* Contributors: DasRoteSkelett, Erick G. Islas-Osuna, Jafar Abdi, Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + +2.1.0 (2022-02-23) +------------------ +* INSTANTIATE_TEST_CASE_P -> INSTANTIATE_TEST_SUITE_P (`#293 `_) +* Contributors: Bence Magyar + +2.0.1 (2022-02-01) +------------------ +* Fix missing control_toolbox dependency (`#291 `_) +* Contributors: Denis Štogl + +2.0.0 (2022-01-28) +------------------ +* [JointTrajectoryController] Add velocity-only command option for JTC with closed loop controller (`#239 `_) + * Add velocity pid support. + * Remove incorrect init test for only velocity command interface. + * Add clarification comments for pid aux variables. Adapt update loop. + * Change dt for pid to appropriate measure. + * Improve partial commands for velocity-only mode. + * Extend tests to use velocity-only mode. + * Increase timeout for velocity-only mode parametrized tests. + * add is_same_sign for better refactor + * refactor boolean logic + * set velocity to 0.0 on deactivate +* Contributors: Lovro Ivanov, Bence Magyar + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ +* Move interface sorting into ControllerInterface (`#259 `_) +* Revise for-loop style (`#254 `_) +* Contributors: bailaC + +1.0.0 (2021-09-29) +------------------ +* Remove compile warnings. (`#245 `_) +* Add time and period to update function (`#241 `_) +* Quickfix 🛠: Correct confusing variable name (`#240 `_) +* Unify style of controllers. (`#236 `_) +* Change test to work with Foxy and posterior action API (`#237 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* refactor get_current_state to get_state (`#232 `_) +* Contributors: Bence Magyar, Denis Štogl, Márk Szitanics, Tyler Weaver, bailaC + +0.5.0 (2021-08-30) +------------------ +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Enable JTC for hardware having offset from state measurements (`#189 `_) + * Avoid "jumps" with states that have tracking error. All test are passing but separatelly. Is there some kind of timeout? + * Remove allow_integration_flag + * Add reading from command interfaces when restarting controller +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ +* Force torque sensor broadcaster (`#152 `_) + * Stabilize joint_trajectory_controller tests + * Add rclcpp::shutdown(); to all standalone test functions +* Fixes for Windows (`#205 `_) + * Export protected joint trajectory controller functions +* Fix deprecation warnings on Rolling, remove rcutils dependency (`#204 `_) +* Fix parameter initialisation for galactic (`#199 `_) + * Fix parameter initialisation for galactic + * Fix forward_command_controller the same way + * Fix other compiler warnings + * Missing space +* Fix rolling build (`#200 `_) + * Fix rolling build + * Stick to printf style + * Add back :: around interface type + Co-authored-by: Bence Magyar +* Contributors: Akash, Bence Magyar, Denis Štogl, Tim Clephas, Vatan Aksoy Tezer + +0.3.1 (2021-05-23) +------------------ +* Reset external trajectory message upon activation (`#185 `_) + * Reset external trajectory message to prevent preserving the old goal on systems with hardware offsets + * Fix has_trajectory_msg() function: two wrongs were making a right so functionally things were fine +* Contributors: Nathan Brooks, Matt Reynolds + +0.3.0 (2021-05-21) +------------------ +* joint_trajectory_controller publishes state in node namespace (`#187 `_) +* [JointTrajectoryController] Enable position, velocity and acceleration interfaces (`#140 `_) + * joint_trajectory_controller should not go into FINALIZED state when fails to configure, remain in UNCONFIGURED +* Contributors: Bence Magyar, Denis Štogl + +0.2.1 (2021-05-03) +------------------ +* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) +* [JTC] Add link to TODOs to provide better trackability (`#169 `_) +* Fix JTC segfault (`#164 `_) + * Use a copy of the rt_active_goal to avoid segfault + * Use RealtimeBuffer for thread-safety +* Add basic user docs pages for each package (`#156 `_) +* Contributors: Bence Magyar, Matt Reynolds + +0.2.0 (2021-02-06) +------------------ +* Use ros2 contol test assets (`#138 `_) + * Add description to test trajecotry_controller + * Use ros2_control_test_assets package + * Delete obsolete components plugin export +* Contributors: Denis Štogl + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ +* Remove lifecycle node controllers (`#124 `_) +* Use resource manager on joint trajectory controller (`#112 `_) +* Use new joint handles in all controllers (`#90 `_) +* More jtc tests (`#75 `_) +* remove unused variables (`#86 `_) +* Port over interpolation formulae, abort if goals tolerance violated (`#62 `_) +* Partial joints (`#68 `_) +* Use clamp function from rcppmath (`#79 `_) +* Reorder incoming out of order joint_names in trajectory messages (`#53 `_) +* Action server for JointTrajectoryController (`#26 `_) +* Add state_publish_rate to JointTrajectoryController (`#25 `_) +* Contributors: Alejandro Hernández Cordero, Anas Abou Allaban, Bence Magyar, Denis Štogl, Edwin Fan, Jordan Palacios, Karsten Knese, Victor Lopez diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt new file mode 100644 index 00000000..f557a138 --- /dev/null +++ b/joint_trajectory_controller/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_trajectory_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rsl + tl_expected + trajectory_msgs +) + +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(joint_trajectory_controller_parameters + src/joint_trajectory_controller_parameters.yaml + include/joint_trajectory_controller/validate_jtc_parameters.hpp +) + +add_library(joint_trajectory_controller SHARED + src/joint_trajectory_controller.cpp + src/trajectory.cpp +) +target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) +target_include_directories(joint_trajectory_controller PUBLIC + $ + $ +) +target_link_libraries(joint_trajectory_controller PUBLIC + joint_trajectory_controller_parameters +) +ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +pluginlib_export_plugin_description_file(controller_interface joint_trajectory_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_trajectory test/test_trajectory.cpp) + target_include_directories(test_trajectory PRIVATE include) + target_link_libraries(test_trajectory ${PROJECT_NAME}) + + ament_add_gmock(test_trajectory_controller + test/test_trajectory_controller.cpp + ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) + set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) + target_include_directories(test_trajectory_controller PRIVATE include) + target_link_libraries(test_trajectory_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_trajectory_controller + ${THIS_PACKAGE_INCLUDE_DEPENDS} + ) + + ament_add_gmock( + test_load_joint_trajectory_controller + test/test_load_joint_trajectory_controller.cpp + ) + target_include_directories(test_load_joint_trajectory_controller PRIVATE include) + ament_target_dependencies(test_load_joint_trajectory_controller + controller_manager + control_toolbox + realtime_tools + ros2_control_test_assets + ) + + # TODO(andyz): Disabled due to flakiness + # ament_add_gmock( + # test_trajectory_actions + # test/test_trajectory_actions.cpp + # ) + # target_include_directories(test_trajectory_actions PRIVATE include) + # target_link_libraries(test_trajectory_actions + # ${PROJECT_NAME} + # ) + # ament_target_dependencies(test_trajectory_actions + # ${THIS_PACKAGE_INCLUDE_DEPENDS} + # ) +endif() + + +install( + DIRECTORY include/ + DESTINATION include/joint_trajectory_controller +) +install(TARGETS + joint_trajectory_controller + joint_trajectory_controller_parameters + EXPORT export_joint_trajectory_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() + diff --git a/joint_trajectory_controller/README.md b/joint_trajectory_controller/README.md new file mode 100644 index 00000000..3d2ebbbb --- /dev/null +++ b/joint_trajectory_controller/README.md @@ -0,0 +1,10 @@ +**This package is from [this Pull Request](https://github.com/ros-controls/ros2_controllers/pull/558/). +This PR contains features to ensure that the current robot configuration is used as a goal when the JTC hasn't already been passed a goal** + +# Original README + +# joint_trajectory_controllers package + +The package implements controllers to interpolate joint's trajectory. + +For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/). diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst new file mode 100644 index 00000000..e3d5c197 --- /dev/null +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -0,0 +1,256 @@ +.. _joint_trajectory_controller_userdoc: + +joint_trajectory_controller +=========================== + +Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. + +Trajectory representation +------------------------- + +The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification: + + Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. + + Cubic: Position and velocity are specified. Guarantees continuity at the velocity level. + + Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. + +Hardware interface type +----------------------- + +The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here. + +Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). + +Other features +-------------- + + Realtime-safe implementation. + + Proper handling of wrapping (continuous) joints. + + Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. + +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +(the controller is not yet implemented as chainable controller) + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Legal combinations of state interfaces are: + +- ``position`` +- ``position`` and ``velocity`` +- ``position``, ``velocity`` and ``acceleration`` +- ``effort`` + +Commands +^^^^^^^^^ + + +Using Joint Trajectory Controller(s) +------------------------------------ + +The controller expects at least position feedback from the hardware. +Joint velocities and accelerations are optional. +Currently the controller does not internally integrate velocity from acceleration and position from velocity. +Therefore if the hardware provides only acceleration or velocity states they have to be integrated in the hardware-interface implementation of velocity and position to use these controllers. + +The generic version of Joint Trajectory controller is implemented in this package. +A yaml file for using it could be: + + .. code-block:: yaml + + controller_manager: + ros__parameters: + joint_trajectory_controller: + type: "joint_trajectory_controller/JointTrajectoryController" + + joint_trajectory_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + action_monitor_rate: 20.0 + + allow_partial_joints_goal: false + open_loop_control: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + joint1: + trajectory: 0.05 + goal: 0.03 + + +Details about parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +joints (list(string)) + Joint names to control and listen to. + +command_joints (list(string)) + Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. + +command_interface (list(string)) + Command interfaces provided by the hardware interface for all joints. + + Values: [position | velocity | acceleration] (multiple allowed) + +state_interfaces (list(string)) + State interfaces provided by the hardware for all joints. + + Values: position (mandatory) [velocity, [acceleration]]. + Acceleration interface can only be used in combination with position and velocity. + +action_monitor_rate (double) + Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). + + Default: 20.0 + +allow_partial_joints_goal (boolean) + Allow joint goals defining trajectory for only some joints. + + Default: false + +allow_integration_in_goal_trajectories (boolean) + Allow integration in goal trajectories to accept goals without position or velocity specified + + Default: false + +interpolation_method (string) + The type of interpolation to use, if any. Can be "splines" or "none". + + Default: splines + +open_loop_control (boolean) + Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + + Default: false + + If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. + +constraints (structure) + Default values for tolerances if no explicit values are states in JointTrajectory message. + +constraints.stopped_velocity_tolerance (double) + Default value for end velocity deviation. + + Default: 0.01 + +constraints.goal_time (double) + Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + + Default: 0.0 (not checked) + +constraints..trajectory (double) + Maximally allowed deviation from the target trajectory for a given joint. + + Default: 0.0 (tolerance is not enforced) + +constraints..goal (double) + Maximally allowed deviation from the goal (end of the trajectory) for a given joint. + + Default: 0.0 (tolerance is not enforced) + +gains (structure) + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. This structure contains the controller gains for every joint with the control law + + .. math:: + + u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + +gains..p (double) + Proportional gain :math:`k_p` for PID + + Default: 0.0 + +gains..i (double) + Integral gain :math:`k_i` for PID + + Default: 0.0 + +gains..d (double) + Derivative gain :math:`k_d` for PID + + Default: 0.0 + +gains..i_clamp (double) + Integral clamp. Symmetrical in both positive and negative direction. + + Default: 0.0 + +gains..ff_velocity_scale (double) + Feed-forward scaling :math:`k_{ff}` of velocity + + Default: 0.0 + +gains..normalize_error (bool) + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface. Use this for revolute joints without end stop, + where the shortest rotation to the target position is the desired motion. + + Default: false + +ROS2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] + Topic for commanding the controller. + +~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] + Topic publishing internal states with the update-rate of the controller manager. + +~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] + Action server for commanding the controller. + + +Specialized versions of JointTrajectoryController (TBD in ...) +-------------------------------------------------------------- + +The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_). + +The following version of the Joint Trajectory Controller are available mapping the following interfaces: + + - position_controllers::JointTrajectoryController + - Input: position, [velocity, [acceleration]] + - Output: position + - position_velocity_controllers::JointTrajectoryController + - Input: position, [velocity, [acceleration]] + - Output: position and velocity + - position_velocity_acceleration_controllers::JointTrajectoryController + - Input: position, [velocity, [acceleration]] + - Output: position, velocity and acceleration + +.. - velocity_controllers::JointTrajectoryController +.. - Input: position, [velocity, [acceleration]] +.. - Output: velocity +.. TODO(anyone): would it be possible to output velocty and acceleration? +.. (to have an vel_acc_controllers) +.. - effort_controllers::JointTrajectoryController +.. - Input: position, [velocity, [acceleration]] +.. - Output: effort + +(*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp new file mode 100644 index 00000000..df8c9da0 --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2022 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace joint_trajectory_controller +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("joint_trajectory_controller.interpolation_methods"); + +namespace interpolation_methods +{ +enum class InterpolationMethod +{ + NONE, + VARIABLE_DEGREE_SPLINE +}; + +const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE; + +const std::unordered_map InterpolationMethodMap( + {{InterpolationMethod::NONE, "none"}, {InterpolationMethod::VARIABLE_DEGREE_SPLINE, "splines"}}); + +[[nodiscard]] inline InterpolationMethod from_string(const std::string & interpolation_method) +{ + if (interpolation_method.compare(InterpolationMethodMap.at(InterpolationMethod::NONE)) == 0) + { + return InterpolationMethod::NONE; + } + else if ( + interpolation_method.compare( + InterpolationMethodMap.at(InterpolationMethod::VARIABLE_DEGREE_SPLINE)) == 0) + { + return InterpolationMethod::VARIABLE_DEGREE_SPLINE; + } + // Default + else + { + RCLCPP_INFO_STREAM( + LOGGER, + "No interpolation method parameter was given. Using the default, VARIABLE_DEGREE_SPLINE."); + return InterpolationMethod::VARIABLE_DEGREE_SPLINE; + } +} +} // namespace interpolation_methods +} // namespace joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp new file mode 100644 index 00000000..2e8b9c6f --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -0,0 +1,267 @@ +// Copyright (c) 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_toolbox/pid.hpp" +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_trajectory_controller/interpolation_methods.hpp" +#include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/visibility_control.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/types.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_server_goal_handle.h" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller_parameters.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace rclcpp_action +{ +template +class ServerGoalHandle; +} // namespace rclcpp_action +namespace rclcpp_lifecycle +{ +class State; +} // namespace rclcpp_lifecycle + +namespace joint_trajectory_controller +{ +class Trajectory; + +class JointTrajectoryController : public controller_interface::ControllerInterface +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + JointTrajectoryController(); + + /** + * @brief command_interface_configuration This controller requires the position command + * interfaces for the controlled joints + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @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::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() 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; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + +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 + trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint state_desired_; + trajectory_msgs::msg::JointTrajectoryPoint state_error_; + + // Degrees of freedom + size_t dof_; + + // Storing command joint names for interfaces + std::vector command_joint_names_; + + // Parameters from ROS for joint_trajectory_controller + std::shared_ptr param_listener_; + Params params_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + /// Specify interpolation method. Default to splines. + interpolation_methods::InterpolationMethod interpolation_method_{ + 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 joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_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_; + // reserved storage for result of the command when closed loop pid adapter is used + std::vector tmp_command_; + + // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr joint_command_subscriber_ = + nullptr; + + std::shared_ptr * traj_point_active_ptr_ = nullptr; + std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr traj_home_point_ptr_ = nullptr; + std::shared_ptr traj_msg_home_ptr_ = nullptr; + realtime_tools::RealtimeBuffer> + traj_msg_external_point_ptr_; + + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; + using StatePublisher = realtime_tools::RealtimePublisher; + using StatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr publisher_; + StatePublisherPtr state_publisher_; + + rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); + rclcpp::Time last_state_publish_time_; + + using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; + 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. + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + // callback for topic interface + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void topic_callback(const std::shared_ptr msg); + + // callbacks for action_server_ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_action::GoalResponse goal_received_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void goal_accepted_callback( + std::shared_ptr> goal_handle); + + // fill trajectory_msg so it matches joints controlled by this controller + // positions set to current position, velocities, accelerations and efforts to 0.0 + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void fill_partial_goal( + std::shared_ptr trajectory_msg) const; + // sorts the joints of the incoming message to our local order + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void sort_to_local_joint_order( + std::shared_ptr trajectory_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void add_new_trajectory_msg( + const std::shared_ptr & traj_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + + SegmentTolerances default_tolerances_; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void preempt_active_goal(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void set_hold_position(); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool reset(); + + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void publish_state( + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error); + + void read_state_from_hardware(JointTrajectoryPoint & state); + + bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + +private: + bool contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type); + + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); +}; + +} // namespace joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ \ No newline at end of file diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp new file mode 100644 index 00000000..de8f060b --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -0,0 +1,180 @@ +// Copyright 2013 PAL Robotics S.L. +// 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 JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "joint_trajectory_controller_parameters.hpp" + +#include "rclcpp/node.hpp" + +namespace joint_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. + */ +SegmentTolerances get_segment_tolerances(Params const & params) +{ + auto const & constraints = params.constraints; + auto const n_joints = params.joints.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.joints[i]; + tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory; + tolerances.goal_state_tolerance[i].position = constraints.joints_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 trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, + const StateTolerances & state_tolerance, bool show_errors = false) +{ + using std::abs; + const double error_position = state_error.positions[joint_idx]; + const double error_velocity = + state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx]; + const double error_acceleration = + state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; + + 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, "Path state tolerances failed:"); + + 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 joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp new file mode 100644 index 00000000..7dac4ddb --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -0,0 +1,189 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ + +#include +#include + +#include "joint_trajectory_controller/interpolation_methods.hpp" +#include "joint_trajectory_controller/visibility_control.h" +#include "rclcpp/time.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" +namespace joint_trajectory_controller +{ +using TrajectoryPointIter = std::vector::iterator; +using TrajectoryPointConstIter = + std::vector::const_iterator; + +class Trajectory +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + Trajectory(); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + explicit Trajectory(std::shared_ptr joint_trajectory); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + explicit Trajectory( + const rclcpp::Time & current_time, + const trajectory_msgs::msg::JointTrajectoryPoint & current_point, + std::shared_ptr joint_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 + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void set_point_before_trajectory_msg( + const rclcpp::Time & current_time, + const trajectory_msgs::msg::JointTrajectoryPoint & current_point); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void update(std::shared_ptr joint_trajectory); + + /// 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. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool sample( + const rclcpp::Time & sample_time, + const interpolation_methods::InterpolationMethod interpolation_method, + trajectory_msgs::msg::JointTrajectoryPoint & output_state, + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + + /** + * 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. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void interpolate_between_points( + const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TrajectoryPointConstIter begin() const; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TrajectoryPointConstIter end() const; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp::Time time_from_start() const; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_trajectory_msg() const; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr get_trajectory_msg() const + { + return trajectory_msg_; + } + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool is_sampled_already() const { return sampled_already_; } + +private: + void deduce_from_derivatives( + trajectory_msgs::msg::JointTrajectoryPoint & first_state, + trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, + const double delta_t); + + std::shared_ptr trajectory_msg_; + rclcpp::Time trajectory_start_time_; + + rclcpp::Time time_before_traj_msg_; + trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; + + bool sampled_already_ = false; +}; + +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 indices. + * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is + * "{2, 1}". + */ +template +inline std::vector mapping(const T & t1, const T & t2) +{ + // t1 must be a subset of t2 + if (t1.size() > t2.size()) + { + return std::vector(); + } + + std::vector mapping_vector(t1.size()); // Return value + for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { + auto t2_it = std::find(t2.begin(), t2.end(), *t1_it); + if (t2.end() == t2_it) + { + return std::vector(); + } + else + { + const size_t t1_dist = std::distance(t1.begin(), t1_it); + const size_t t2_dist = std::distance(t2.begin(), t2_it); + mapping_vector[t1_dist] = t2_dist; + } + } + return mapping_vector; +} + +} // namespace joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp new file mode 100644 index 00000000..fc6319fa --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2022 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ + +#include +#include + +#include "parameter_traits/parameter_traits.hpp" +#include "rclcpp/parameter.hpp" +#include "rsl/algorithm.hpp" +#include "tl_expected/expected.hpp" + +namespace parameter_traits +{ +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' " + "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' 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 parameter_traits + +#endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h b/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h new file mode 100644 index 00000000..ec44da7a --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ +#define JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((dllexport)) +#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec(dllexport) +#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT +#else +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_LOCAL +#else +#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_LOCAL +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/joint_trajectory_controller/joint_trajectory_plugin.xml b/joint_trajectory_controller/joint_trajectory_plugin.xml new file mode 100644 index 00000000..27930380 --- /dev/null +++ b/joint_trajectory_controller/joint_trajectory_plugin.xml @@ -0,0 +1,7 @@ + + + + The joint trajectory controller executes joint-space trajectories on a set of joints + + + diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml new file mode 100644 index 00000000..7c1f9c74 --- /dev/null +++ b/joint_trajectory_controller/package.xml @@ -0,0 +1,41 @@ + + + joint_trajectory_controller + 3.3.0 + Controller for executing joint-space trajectories on a group of joints + Bence Magyar + Jordan Palacios + Karsten Knese + + Apache License 2.0 + + ament_cmake + + angles + pluginlib + realtime_tools + + angles + pluginlib + realtime_tools + + backward_ros + controller_interface + control_msgs + control_toolbox + generate_parameter_library + hardware_interface + rclcpp + rclcpp_lifecycle + rsl + tl_expected + trajectory_msgs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp new file mode 100644 index 00000000..24a00944 --- /dev/null +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -0,0 +1,1312 @@ +// Copyright (c) 2021 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 "joint_trajectory_controller/joint_trajectory_controller.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_trajectory_controller/trajectory.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_event.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/header.hpp" + +namespace joint_trajectory_controller +{ +JointTrajectoryController::JointTrajectoryController() +: controller_interface::ControllerInterface(), dof_(0) +{ +} + +controller_interface::CallbackReturn JointTrajectoryController::on_init() +{ + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + // Set interpolation method from string parameter + interpolation_method_ = interpolation_methods::from_string(params_.interpolation_method); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +JointTrajectoryController::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 & joint_name : command_joint_names_) + { + for (const auto & interface_type : params_.command_interfaces) + { + conf.names.push_back(joint_name + "/" + interface_type); + } + } + return conf; +} + +controller_interface::InterfaceConfiguration +JointTrajectoryController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(dof_ * params_.state_interfaces.size()); + for (const auto & joint_name : params_.joints) + { + for (const auto & interface_type : params_.state_interfaces) + { + conf.names.push_back(joint_name + "/" + interface_type); + } + } + return conf; +} + +controller_interface::return_type JointTrajectoryController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + return controller_interface::return_type::OK; + } + + auto compute_error_for_joint = [&]( + JointTrajectoryPoint & error, int index, + const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) + { + // error defined as the difference between current and desired + error.positions[index] = + angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + if (has_velocity_state_interface_ && has_velocity_command_interface_) + { + error.velocities[index] = desired.velocities[index] - current.velocities[index]; + } + if (has_acceleration_state_interface_ && has_acceleration_command_interface_) + { + error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + } + }; + + // Check if a new external message has been received from nonRT threads + auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); + auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + if (current_external_msg != *new_external_msg) + { + fill_partial_goal(*new_external_msg); + sort_to_local_joint_order(*new_external_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(*new_external_msg); + } + + // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not + // changed, but its value only? + auto assign_interface_from_point = + [&](auto & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; + + // current state update + state_current_.time_from_start.set__sec(0); + read_state_from_hardware(state_current_); + + // currently carrying out a trajectory + if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + { + bool first_sample = false; + // if sampling the first time, set the point before you sample + if (!(*traj_point_active_ptr_)->is_sampled_already()) + { + first_sample = true; + if (params_.open_loop_control) + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + } + else + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); + } + } + + // find segment for current timestamp + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + const bool valid_point = + (*traj_point_active_ptr_) + ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + + if (valid_point) + { + bool tolerance_violated_while_moving = false; + bool outside_goal_tolerance = false; + bool within_goal_time = true; + double time_difference = 0.0; + const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); + + // Check state/goal tolerance + for (size_t index = 0; index < dof_; ++index) + { + compute_error_for_joint(state_error_, index, state_current_, state_desired_); + + // Always check the state tolerance on the first sample in case the first sample + // is the last point + if ( + (before_last_point || first_sample) && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.state_tolerance[index], false)) + { + tolerance_violated_while_moving = true; + } + // past the final point, check that we end up inside goal tolerance + if ( + !before_last_point && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + { + outside_goal_tolerance = true; + + if (default_tolerances_.goal_time_tolerance != 0.0) + { + // if we exceed goal_time_tolerance set it to aborted + const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); + const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; + + time_difference = get_node()->now().seconds() - traj_end.seconds(); + + if (time_difference > default_tolerances_.goal_time_tolerance) + { + within_goal_time = false; + } + } + } + } + + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) + { + // Update PIDs + for (auto i = 0ul; i < dof_; ++i) + { + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_desired_.positions[i] - state_current_.positions[i], + state_desired_.velocities[i] - state_current_.velocities[i], + (uint64_t)period.nanoseconds()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) + { + // send feedback + auto feedback = std::make_shared(); + feedback->header.stamp = time; + feedback->joint_names = params_.joints; + + feedback->actual = state_current_; + feedback->desired = state_desired_; + feedback->error = state_error_; + active_goal->setFeedback(feedback); + + // check abort + if (tolerance_violated_while_moving) + { + set_hold_position(); + auto result = std::make_shared(); + + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + 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()); + + // check goal tolerance + } + else if (!before_last_point) + { + if (!outside_goal_tolerance) + { + auto res = std::make_shared(); + res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + active_goal->setSucceeded(res); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + + RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + } + else if (!within_goal_time) + { + set_hold_position(); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + 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()); + RCLCPP_WARN( + get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + time_difference); + } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied or violated within the goal_time_tolerance + } + } + else if (tolerance_violated_while_moving) + { + set_hold_position(); + RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + } + } + } + else + { + // TODO(peterdavidfagan) temporary solution to keep the initial position + // before the first trajectory goal arrives + set_hold_position(); + } + + publish_state(state_desired_, state_current_, state_error_); + return controller_interface::return_type::OK; +} + +void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +{ + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + // Assign values from the hardware + // Position states always exist + assign_point_from_interface(state.positions, joint_state_interface_[0]); + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + assign_point_from_interface(state.velocities, joint_state_interface_[1]); + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) + { + assign_point_from_interface(state.accelerations, joint_state_interface_[2]); + } + else + { + // Make empty so the property is ignored during interpolation + state.accelerations.clear(); + } + } + else + { + // Make empty so the property is ignored during interpolation + state.velocities.clear(); + state.accelerations.clear(); + } +} + +bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) +{ + bool has_values = true; + + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + 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. Therefore needs check for both. + // Position state interface has to exist always + if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) + { + assign_point_from_interface(state.positions, joint_command_interface_[0]); + } + else + { + state.positions.clear(); + has_values = false; + } + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) + { + assign_point_from_interface(state.velocities, joint_command_interface_[1]); + } + else + { + state.velocities.clear(); + has_values = false; + } + } + else + { + state.velocities.clear(); + } + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) + { + if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) + { + assign_point_from_interface(state.accelerations, joint_command_interface_[2]); + } + else + { + state.accelerations.clear(); + has_values = false; + } + } + else + { + state.accelerations.clear(); + } + + return has_values; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + 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(); + + // Check if the DoF has changed + if ((dof_ > 0) && (params_.joints.size() != dof_)) + { + RCLCPP_ERROR( + logger, + "The JointTrajectoryController does not support restarting with a different number of DOF"); + // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we + // can continue + return CallbackReturn::FAILURE; + } + + dof_ = params_.joints.size(); + + // TODO(destogl): why is this here? Add comment or move + if (!reset()) + { + return CallbackReturn::FAILURE; + } + + if (params_.joints.empty()) + { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? + RCLCPP_WARN(logger, "'joints' parameter is empty."); + } + + command_joint_names_ = params_.command_joints; + + if (command_joint_names_.empty()) + { + command_joint_names_ = params_.joints; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != params_.joints.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + + // // Specialized, child controllers set interfaces before calling configure function. + // if (command_interface_types_.empty()) + // { + // command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); + // } + + 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 + joint_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); + + // if there is only velocity or if there is effort command interface + // then use also PID adapter + use_closed_loop_pid_adapter_ = + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1) || + has_effort_command_interface_; + + if (use_closed_loop_pid_adapter_) + { + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + tmp_command_.resize(dof_, 0.0); + + // Init PID gains from ROS parameters + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + + // TODO(destogl): remove this in ROS2 Iron + // Check deprecated style for "ff_velocity_scale" parameter definition. + if (gains.ff_velocity_scale == 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " + "Maybe you are using deprecated format 'ff_velocity_scale/'!"); + + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + } + else + { + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + } + } + + if (params_.state_interfaces.empty()) + { + RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); + 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. + joint_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()); + + default_tolerances_ = get_segment_tolerances(params_); + + const std::string interpolation_string = + get_node()->get_parameter("interpolation_method").as_string(); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + RCLCPP_INFO( + logger, "Using '%s' interpolation method.", + interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + + joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), + std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); + + // State publisher + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", params_.state_publish_rate); + if (params_.state_publish_rate > 0.0) + { + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / params_.state_publish_rate); + } + else + { + state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); + } + + publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(publisher_); + + state_publisher_->lock(); + state_publisher_->msg_.joint_names = params_.joints; + state_publisher_->msg_.desired.positions.resize(dof_); + state_publisher_->msg_.desired.velocities.resize(dof_); + state_publisher_->msg_.desired.accelerations.resize(dof_); + state_publisher_->msg_.actual.positions.resize(dof_); + state_publisher_->msg_.error.positions.resize(dof_); + if (has_velocity_state_interface_) + { + state_publisher_->msg_.actual.velocities.resize(dof_); + state_publisher_->msg_.error.velocities.resize(dof_); + } + if (has_acceleration_state_interface_) + { + state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.error.accelerations.resize(dof_); + } + state_publisher_->unlock(); + + last_state_publish_time_ = get_node()->now(); + + // action server configuration + if (params_.allow_partial_joints_goal) + { + RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); + } + + 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(&JointTrajectoryController::goal_received_callback, this, _1, _2), + std::bind(&JointTrajectoryController::goal_cancelled_callback, this, _1), + std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); + + resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point(state_desired_, dof_); + resize_joint_trajectory_point(state_error_, dof_); + resize_joint_trajectory_point(last_commanded_state_, dof_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointTrajectoryController::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 CallbackReturn::ERROR; + } + } + for (const auto & interface : params_.state_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( + state_interfaces_, params_.joints, 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 CallbackReturn::ERROR; + } + } + + // Store 'home' pose + traj_msg_home_ptr_ = std::make_shared(); + traj_msg_home_ptr_->header.stamp.sec = 0; + traj_msg_home_ptr_->header.stamp.nanosec = 0; + traj_msg_home_ptr_->points.resize(1); + traj_msg_home_ptr_->points[0].time_from_start.sec = 0; + traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; + traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); + for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) + { + traj_msg_home_ptr_->points[0].positions[index] = + joint_state_interface_[0][index].get().get_value(); + } + + traj_external_point_ptr_ = std::make_shared(); + traj_home_point_ptr_ = std::make_shared(); + traj_msg_external_point_ptr_.writeFromNonRT( + std::shared_ptr()); + + subscriber_is_active_ = true; + traj_point_active_ptr_ = &traj_external_point_ptr_; + last_state_publish_time_ = get_node()->now(); + + // Initialize current state storage if hardware state has tracking offset + read_state_from_hardware(state_current_); + read_state_from_hardware(state_desired_); + read_state_from_hardware(last_commanded_state_); + // Handle restart of controller by reading from commands if + // those are not nan + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + if (read_state_from_command_interfaces(state)) + { + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + // TODO(anyone): How to halt when using effort commands? + for (size_t index = 0; index < dof_; ++index) + { + if (has_position_command_interface_) + { + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); + } + + if (has_velocity_command_interface_) + { + joint_command_interface_[1][index].get().set_value(0.0); + } + + if (has_effort_command_interface_) + { + joint_command_interface_[3][index].get().set_value(0.0); + } + } + + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); + + subscriber_is_active_ = false; + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + // go home + if (traj_home_point_ptr_ != nullptr) + { + traj_home_point_ptr_->update(traj_msg_home_ptr_); + traj_point_active_ptr_ = &traj_home_point_ptr_; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_error( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +bool JointTrajectoryController::reset() +{ + subscriber_is_active_ = false; + joint_command_subscriber_.reset(); + + for (const auto & pid : pids_) + { + if (pid) + { + pid->reset(); + } + } + + // iterator has no default value + // prev_traj_point_ptr_; + traj_point_active_ptr_ = nullptr; + traj_external_point_ptr_.reset(); + traj_home_point_ptr_.reset(); + traj_msg_home_ptr_.reset(); + + return true; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( + const rclcpp_lifecycle::State &) +{ + // TODO(karsten1987): what to do? + + return CallbackReturn::SUCCESS; +} + +void JointTrajectoryController::publish_state( + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error) +{ + if (state_publisher_period_.seconds() <= 0.0) + { + return; + } + + if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) + { + return; + } + + if (state_publisher_ && state_publisher_->trylock()) + { + last_state_publish_time_ = get_node()->now(); + state_publisher_->msg_.header.stamp = last_state_publish_time_; + state_publisher_->msg_.desired.positions = desired_state.positions; + state_publisher_->msg_.desired.velocities = desired_state.velocities; + state_publisher_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_->msg_.actual.positions = current_state.positions; + state_publisher_->msg_.error.positions = state_error.positions; + if (has_velocity_state_interface_) + { + state_publisher_->msg_.actual.velocities = current_state.velocities; + state_publisher_->msg_.error.velocities = state_error.velocities; + } + if (has_acceleration_state_interface_) + { + state_publisher_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_->msg_.error.accelerations = state_error.accelerations; + } + + state_publisher_->unlockAndPublish(); + } +} + +void JointTrajectoryController::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); + } +}; + +rclcpp_action::GoalResponse JointTrajectoryController::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_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 JointTrajectoryController::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) + { + // Controller uptime + // Enter hold current position mode + set_hold_position(); + + RCLCPP_DEBUG( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void JointTrajectoryController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + // Update new trajectory + { + preempt_active_goal(); + auto traj_msg = + std::make_shared(goal_handle->get_goal()->trajectory); + + add_new_trajectory_msg(traj_msg); + } + + // Update the active goal + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->preallocated_feedback_->joint_names = params_.joints; + 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 JointTrajectoryController::fill_partial_goal( + std::shared_ptr trajectory_msg) const +{ + // joint names in the goal are a subset of existing joints, as checked in goal_callback + // so if the size matches, the goal contains all controller joints + if (dof_ == trajectory_msg->joint_names.size()) + { + return; + } + + trajectory_msg->joint_names.reserve(dof_); + + for (size_t index = 0; index < dof_; ++index) + { + { + if ( + std::find( + trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), + params_.joints[index]) != trajectory_msg->joint_names.end()) + { + // joint found on msg + continue; + } + trajectory_msg->joint_names.push_back(params_.joints[index]); + + for (auto & it : trajectory_msg->points) + { + // Assume hold position with 0 velocity and acceleration for missing joints + if (!it.positions.empty()) + { + if ( + has_position_command_interface_ && + !std::isnan(joint_command_interface_[0][index].get().get_value())) + { + // copy last command if cmd interface exists + it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + } + else if (has_position_state_interface_) + { + // copy current state if state interface exists + it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + } + } + if (!it.velocities.empty()) + { + it.velocities.push_back(0.0); + } + if (!it.accelerations.empty()) + { + it.accelerations.push_back(0.0); + } + if (!it.effort.empty()) + { + it.effort.push_back(0.0); + } + } + } + } +} + +void JointTrajectoryController::sort_to_local_joint_order( + std::shared_ptr trajectory_msg) +{ + // rearrange all points in the trajectory message based on mapping + std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); + auto remap = [this]( + const std::vector & to_remap, + const std::vector & mapping) -> std::vector + { + if (to_remap.empty()) + { + return to_remap; + } + if (to_remap.size() != mapping.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + return to_remap; + } + std::vector output; + output.resize(mapping.size(), 0.0); + for (size_t index = 0; index < mapping.size(); ++index) + { + auto map_index = mapping[index]; + output[map_index] = to_remap[index]; + } + return output; + }; + + for (size_t index = 0; index < trajectory_msg->points.size(); ++index) + { + trajectory_msg->points[index].positions = + remap(trajectory_msg->points[index].positions, mapping_vector); + + trajectory_msg->points[index].velocities = + remap(trajectory_msg->points[index].velocities, mapping_vector); + + trajectory_msg->points[index].accelerations = + remap(trajectory_msg->points[index].accelerations, mapping_vector); + + trajectory_msg->points[index].effort = + remap(trajectory_msg->points[index].effort, mapping_vector); + } +} + +bool JointTrajectoryController::validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const +{ + if (allow_empty && vector_field.empty()) + { + return true; + } + if (joint_names_size != vector_field.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + return false; + } + return true; +} + +bool JointTrajectoryController::validate_trajectory_msg( + const trajectory_msgs::msg::JointTrajectory & trajectory) const +{ + // If partial joints goals are not allowed, goal should specify all controller joints + if (!params_.allow_partial_joints_goal) + { + if (trajectory.joint_names.size() != dof_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Joints on incoming trajectory don't match the controller joints."); + return false; + } + } + + if (trajectory.joint_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); + return false; + } + + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) + { + trajectory_end_time += p.time_from_start; + } + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non zero time start time (%f) that ends on the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + + for (size_t i = 0; i < trajectory.joint_names.size(); ++i) + { + const std::string & incoming_joint_name = trajectory.joint_names[i]; + + auto it = std::find(params_.joints.begin(), params_.joints.end(), incoming_joint_name); + if (it == params_.joints.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", + incoming_joint_name.c_str()); + return false; + } + } + + rclcpp::Duration previous_traj_time(0ms); + for (size_t i = 0; i < trajectory.points.size(); ++i) + { + if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", + i - 1, i, previous_traj_time.seconds(), + rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); + return false; + } + previous_traj_time = trajectory.points[i].time_from_start; + + const size_t joint_count = trajectory.joint_names.size(); + const auto & points = trajectory.points; + // This currently supports only position, velocity and acceleration inputs + if (params_.allow_integration_in_goal_trajectories) + { + const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty(); + const bool position_error = + !points[i].positions.empty() && + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); + const bool velocity_error = + !points[i].velocities.empty() && + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); + const bool acceleration_error = + !points[i].accelerations.empty() && + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, false); + if (all_empty || position_error || velocity_error || acceleration_error) + { + return false; + } + } + else if ( + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, true) || + !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) + { + return false; + } + } + return true; +} + +void JointTrajectoryController::add_new_trajectory_msg( + const std::shared_ptr & traj_msg) +{ + traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); +} + +void JointTrajectoryController::preempt_active_goal() +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + set_hold_position(); + 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()); + } +} + +void JointTrajectoryController::set_hold_position() +{ + trajectory_msgs::msg::JointTrajectory msg; + msg.header.stamp = rclcpp::Time(0); + msg.joint_names = params_.joints; + trajectory_msgs::msg::JointTrajectoryPoint point; + for (const auto & joint_position : last_commanded_state_.positions) + { + point.velocities.push_back(0); + point.accelerations.push_back(0); + point.positions.push_back(joint_position); + } + msg.points.push_back(point); + + auto traj_msg = std::make_shared(msg); + add_new_trajectory_msg(traj_msg); +} + +bool JointTrajectoryController::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(); +} + +void JointTrajectoryController::resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_state_interface_) + { + point.accelerations.resize(size, 0.0); + } +} + +} // namespace joint_trajectory_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml new file mode 100644 index 00000000..b6b6eff0 --- /dev/null +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -0,0 +1,133 @@ +joint_trajectory_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller", + validation: { + unique<>: null, + } + } + command_joints: { + type: string_array, + default_value: [], + description: "Names of the commanding joints used by the controller", + validation: { + unique<>: null, + } + } + command_interfaces: { + type: string_array, + default_value: [], + description: "Names of command interfaces to claim", + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration", "effort",]], + command_interface_type_combinations: null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "Names of state interfaces to claim", + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration",]], + state_interface_type_combinations: null, + } + } + state_publish_rate: { + type: double, + default_value: 50.0, + description: "Rate controller state is published", + validation: { + gt_eq: [0.1] + } + } + allow_partial_joints_goal: { + type: bool, + default_value: false, + description: "Goals with partial set of joints are allowed", + } + open_loop_control: { + type: bool, + default_value: false, + description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + } + 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 status changes will be monitored", + validation: { + gt_eq: [0.1] + } + } + interpolation_method: { + type: string, + default_value: "splines", + description: "The type of interpolation to use, if any", + validation: { + one_of<>: [["splines", "none"]], + } + } + gains: + __map_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Intigral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integrale clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling of velocity." + } + normalize_error: { + type: bool, + default_value: false, + description: "Use position error normalization 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.", + validation: { + gt_eq: [0.0], + } + } + __map_joints: + trajectory: { + type: double, + default_value: 0.0, + description: "Per-joint trajectory offset tolerance during movement.", + } + goal: { + type: double, + default_value: 0.0, + description: "Per-joint trajectory offset tolerance at the goal position.", + } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp new file mode 100644 index 00000000..06b3ca6e --- /dev/null +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -0,0 +1,354 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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/trajectory.hpp" + +#include + +#include "hardware_interface/macros.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "std_msgs/msg/header.hpp" + +namespace joint_trajectory_controller +{ +Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} + +Trajectory::Trajectory(std::shared_ptr joint_trajectory) +: trajectory_msg_(joint_trajectory), + trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) +{ +} + +Trajectory::Trajectory( + const rclcpp::Time & current_time, + const trajectory_msgs::msg::JointTrajectoryPoint & 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(current_time, current_point); + update(joint_trajectory); +} + +void Trajectory::set_point_before_trajectory_msg( + const rclcpp::Time & current_time, + const trajectory_msgs::msg::JointTrajectoryPoint & current_point) +{ + time_before_traj_msg_ = current_time; + state_before_traj_msg_ = current_point; +} + +void Trajectory::update(std::shared_ptr joint_trajectory) +{ + trajectory_msg_ = joint_trajectory; + trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); + sampled_already_ = false; +} + +bool Trajectory::sample( + const rclcpp::Time & sample_time, + const interpolation_methods::InterpolationMethod interpolation_method, + trajectory_msgs::msg::JointTrajectoryPoint & output_state, + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) +{ + THROW_ON_NULLPTR(trajectory_msg_) + + if (trajectory_msg_->points.empty()) + { + start_segment_itr = end(); + end_segment_itr = end(); + return false; + } + + // first sampling of this trajectory + if (!sampled_already_) + { + if (trajectory_start_time_.seconds() == 0.0) + { + trajectory_start_time_ = sample_time; + } + + sampled_already_ = true; + } + + // sampling before the current point + if (sample_time < time_before_traj_msg_) + { + return false; + } + + output_state = trajectory_msgs::msg::JointTrajectoryPoint(); + auto & first_point_in_msg = trajectory_msg_->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 == interpolation_methods::InterpolationMethod::NONE) + { + output_state = 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_, first_point_in_msg, state_before_traj_msg_.positions.size(), + (first_point_timestamp - time_before_traj_msg_).seconds()); + + interpolate_between_points( + time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, + sample_time, output_state); + } + start_segment_itr = begin(); // no segments before the first + end_segment_itr = begin(); + return true; + } + + // time_from_start + trajectory time is the expected arrival time of trajectory + const auto last_idx = trajectory_msg_->points.size() - 1; + for (size_t i = 0; i < last_idx; ++i) + { + auto & point = trajectory_msg_->points[i]; + auto & next_point = trajectory_msg_->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 == interpolation_methods::InterpolationMethod::NONE) + { + output_state = 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, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); + + interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); + } + start_segment_itr = begin() + i; + end_segment_itr = begin() + (i + 1); + return true; + } + } + + // whole animation has played out + start_segment_itr = --end(); + end_segment_itr = end(); + output_state = (*start_segment_itr); + // the trajectories in msg may have empty velocities/accel, so resize them + if (output_state.velocities.empty()) + { + output_state.velocities.resize(output_state.positions.size(), 0.0); + } + if (output_state.accelerations.empty()) + { + output_state.accelerations.resize(output_state.positions.size(), 0.0); + } + return true; +} + +void Trajectory::interpolate_between_points( + const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output) +{ + rclcpp::Duration duration_so_far = sample_time - time_a; + rclcpp::Duration duration_btwn_points = time_b - time_a; + + const size_t dim = state_a.positions.size(); + output.positions.resize(dim, 0.0); + output.velocities.resize(dim, 0.0); + output.accelerations.resize(dim, 0.0); + + 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; + } + }; + + bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty(); + bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty(); + if (duration_so_far.seconds() < 0.0) + { + duration_so_far = rclcpp::Duration::from_seconds(0.0); + has_velocity = has_accel = false; + } + if (duration_so_far.seconds() > duration_btwn_points.seconds()) + { + duration_so_far = duration_btwn_points; + has_velocity = has_accel = false; + } + + double t[6]; + generate_powers(5, duration_so_far.seconds(), t); + + if (!has_velocity && !has_accel) + { + // do linear interpolation + for (size_t i = 0; i < dim; ++i) + { + double start_pos = state_a.positions[i]; + double end_pos = state_b.positions[i]; + + 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.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1]; + output.velocities[i] = t[0] * coefficients[1]; + } + } + else if (has_velocity && !has_accel) + { + // do cubic interpolation + double T[4]; + generate_powers(3, duration_btwn_points.seconds(), T); + + for (size_t i = 0; i < dim; ++i) + { + double start_pos = state_a.positions[i]; + double start_vel = state_a.velocities[i]; + double end_pos = state_b.positions[i]; + double end_vel = state_b.velocities[i]; + + 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.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] + + t[2] * coefficients[2] + t[3] * coefficients[3]; + output.velocities[i] = + t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + t[2] * 3.0 * coefficients[3]; + output.accelerations[i] = 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); + + for (size_t i = 0; i < dim; ++i) + { + double start_pos = state_a.positions[i]; + double start_vel = state_a.velocities[i]; + double start_acc = state_a.accelerations[i]; + double end_pos = state_b.positions[i]; + double end_vel = state_b.velocities[i]; + double end_acc = state_b.accelerations[i]; + + 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.positions[i] = 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.velocities[i] = 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.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3] + + t[2] * 12.0 * coefficients[4] + t[3] * 20.0 * coefficients[5]; + } + } +} + +void Trajectory::deduce_from_derivatives( + trajectory_msgs::msg::JointTrajectoryPoint & first_state, + trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t) +{ + if (second_state.positions.empty()) + { + second_state.positions.resize(dim); + if (first_state.velocities.empty()) + { + first_state.velocities.resize(dim, 0.0); + } + if (second_state.velocities.empty()) + { + second_state.velocities.resize(dim); + if (first_state.accelerations.empty()) + { + first_state.accelerations.resize(dim, 0.0); + } + for (size_t i = 0; i < dim; ++i) + { + second_state.velocities[i] = + first_state.velocities[i] + + (first_state.accelerations[i] + second_state.accelerations[i]) * 0.5 * delta_t; + } + } + for (size_t i = 0; i < dim; ++i) + { + // second state velocity should be reached on the end of the segment, so use middle + second_state.positions[i] = + first_state.positions[i] + + (first_state.velocities[i] + second_state.velocities[i]) * 0.5 * delta_t; + } + } +} + +TrajectoryPointConstIter Trajectory::begin() const +{ + THROW_ON_NULLPTR(trajectory_msg_) + + return trajectory_msg_->points.begin(); +} + +TrajectoryPointConstIter Trajectory::end() const +{ + THROW_ON_NULLPTR(trajectory_msg_) + + return trajectory_msg_->points.end(); +} + +rclcpp::Time Trajectory::time_from_start() const { return trajectory_start_time_; } + +bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } + +} // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml new file mode 100644 index 00000000..09a134e0 --- /dev/null +++ b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml @@ -0,0 +1,12 @@ +test_joint_trajectory_controller: + joints: + - joint1 + - joint2 + - joint3 + + command_interfaces: + - position + + state_interfaces: + - position + - velocity diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp new file mode 100644 index 00000000..0f94d0b1 --- /dev/null +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.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 "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_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController")); + + rclcpp::shutdown(); +} diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp new file mode 100644 index 00000000..b52aa67a --- /dev/null +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -0,0 +1,823 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "joint_trajectory_controller/trajectory.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "std_msgs/msg/header.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +using namespace joint_trajectory_controller::interpolation_methods; // NOLINT +using namespace std::chrono_literals; + +namespace +{ +// Floating-point value comparison threshold +const double EPS = 1e-8; +} // namespace + +TEST(TestTrajectory, initialize_trajectory) +{ + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + { + auto empty_msg = std::make_shared(); + empty_msg->header.stamp.sec = 2; + empty_msg->header.stamp.nanosec = 2; + const rclcpp::Time empty_time = empty_msg->header.stamp; + auto traj = joint_trajectory_controller::Trajectory(empty_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_point; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + + EXPECT_EQ(traj.end(), start); + EXPECT_EQ(traj.end(), end); + EXPECT_EQ(empty_time, traj.time_from_start()); + } + { + auto empty_msg = std::make_shared(); + empty_msg->header.stamp.sec = 0; + empty_msg->header.stamp.nanosec = 0; + const auto now = clock.now(); + auto traj = joint_trajectory_controller::Trajectory(empty_msg); + const auto traj_starttime = traj.time_from_start(); + + trajectory_msgs::msg::JointTrajectoryPoint expected_point; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + + EXPECT_EQ(traj.end(), start); + EXPECT_EQ(traj.end(), end); + const auto allowed_delta = 10000ll; + EXPECT_LT(traj.time_from_start().nanoseconds() - now.nanoseconds(), allowed_delta); + } +} + +TEST(TestTrajectory, sample_trajectory_positions) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + double duration_first_seg = 1.0; + double velocity = (p1.positions[0] - point_before_msg.positions[0]) / duration_first_seg; + + // sample at trajectory starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(velocity, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; + EXPECT_NEAR(half_current_to_p1, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // sample 1s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(velocity, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; + EXPECT_NEAR(half_p1_to_p2, expected_state.positions[0], EPS); + } + + // sample 2.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; + EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); + } + + // sample 3s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample past given points + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, + start, end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample long past given points for same trajectory, it should receive the same end point + // so later in the query_state_service we set it to failure + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } +} + +TEST(TestTrajectory, interpolation_pos_vel) +{ + // taken from ros1_controllers QuinticSplineSegmentTest::PosVelEnpointsSampler + + // Start and end state taken from x^3 - 2x + trajectory_msgs::msg::JointTrajectoryPoint start_state; + start_state.time_from_start = rclcpp::Duration::from_seconds(1.0); + start_state.positions.push_back(0.0); + start_state.velocities.push_back(-2.0); + start_state.accelerations.clear(); + + trajectory_msgs::msg::JointTrajectoryPoint end_state; + end_state.time_from_start = rclcpp::Duration::from_seconds(3.0); + end_state.positions.push_back(4.0); + end_state.velocities.push_back(10.0); + end_state.accelerations.push_back(0.0); // Should be ignored, start state does not specify it + + auto traj = joint_trajectory_controller::Trajectory(); + rclcpp::Time time_now(0); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + + // sample at start_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start, expected_state); + EXPECT_NEAR(start_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(start_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample at mid-segment: Zero-crossing + { + auto t = rclcpp::Duration::from_seconds(std::sqrt(2.0)); + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start + t, expected_state); + EXPECT_NEAR(0.0, expected_state.positions[0], EPS); + EXPECT_NEAR(4.0, expected_state.velocities[0], EPS); + EXPECT_NEAR(6.0 * std::sqrt(2.0), expected_state.accelerations[0], EPS); + } + + // sample at end_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + end_state.time_from_start, expected_state); + EXPECT_NEAR(end_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(end_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(12.0, expected_state.accelerations[0], EPS); + } +} + +TEST(TestTrajectory, interpolation_pos_vel_accel) +{ + // taken from ros1_controllers QuinticSplineSegmentTest::PosVeAcclEnpointsSampler + + // Start and end state taken from x(x-1)(x-2)(x-3)(x-4) = x^5 -10x^4 + 35x^3 -50x^2 + 24x + trajectory_msgs::msg::JointTrajectoryPoint start_state; + start_state.time_from_start = rclcpp::Duration::from_seconds(1.0); + start_state.positions.push_back(0.0); + start_state.velocities.push_back(24.0); + start_state.accelerations.push_back(-100.0); + + trajectory_msgs::msg::JointTrajectoryPoint end_state; + end_state.time_from_start = rclcpp::Duration::from_seconds(3.0); + end_state.positions.push_back(0.0); + end_state.velocities.push_back(4.0); + end_state.accelerations.push_back(0.0); + + auto traj = joint_trajectory_controller::Trajectory(); + rclcpp::Time time_now(0); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + + // sample at start_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start, expected_state); + EXPECT_NEAR(start_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(start_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(start_state.accelerations[0], expected_state.accelerations[0], EPS); + } + + // Sample at mid-segment: Zero-crossing + { + auto t = rclcpp::Duration::from_seconds(1.0); + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start + t, expected_state); + EXPECT_NEAR(0.0, expected_state.positions[0], EPS); + EXPECT_NEAR(-6.0, expected_state.velocities[0], EPS); + EXPECT_NEAR(10.0, expected_state.accelerations[0], EPS); + } + + // sample at end_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + end_state.time_from_start, expected_state); + EXPECT_NEAR(end_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(end_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(end_state.accelerations[0], expected_state.accelerations[0], EPS); + } +} + +TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double time_third_seg = 3.0; + double velocity_first_seg = 1.0; + double velocity_second_seg = 2.0; + double velocity_third_seg = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.velocities.push_back(velocity_first_seg); + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.velocities.push_back(velocity_second_seg); + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.velocities.push_back(velocity_third_seg); + p3.time_from_start = rclcpp::Duration::from_seconds(time_third_seg); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + point_before_msg.velocities.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(point_before_msg.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR((velocity_first_seg / time_first_seg), expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + double half_current_to_p1 = + point_before_msg.positions[0] + + (point_before_msg.velocities[0] + + ((point_before_msg.velocities[0] + p1.velocities[0]) / 2 - point_before_msg.velocities[0]) / + 2) * + 0.5; + EXPECT_NEAR(half_current_to_p1, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0] / 2, expected_state.velocities[0], EPS); + EXPECT_NEAR((velocity_first_seg / time_first_seg), expected_state.accelerations[0], EPS); + } + + // sample 1s after msg + double position_first_seg = + point_before_msg.positions[0] + (0.0 + p1.velocities[0]) / 2 * time_first_seg; + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_second_seg - velocity_first_seg / (time_second_seg - time_first_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + double half_p1_to_p2 = + position_first_seg + + (p1.velocities[0] + ((p1.velocities[0] + p2.velocities[0]) / 2 - p1.velocities[0]) / 2) * 0.5; + EXPECT_NEAR(half_p1_to_p2, expected_state.positions[0], EPS); + double half_p1_to_p2_vel = (p1.velocities[0] + p2.velocities[0]) / 2; + EXPECT_NEAR(half_p1_to_p2_vel, expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_second_seg - velocity_first_seg / (time_second_seg - time_first_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 2s after msg + double position_second_seg = position_first_seg + (p1.velocities[0] + p2.velocities[0]) / 2 * + (time_second_seg - time_first_seg); + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ((++traj.begin()), start); + EXPECT_EQ((--traj.end()), end); + EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p2.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_third_seg - velocity_second_seg / (time_third_seg - time_second_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 2.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ((++traj.begin()), start); + EXPECT_EQ((--traj.end()), end); + double half_p2_to_p3 = + position_second_seg + + (p2.velocities[0] + ((p2.velocities[0] + p3.velocities[0]) / 2 - p2.velocities[0]) / 2) * 0.5; + EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); + double half_p2_to_p3_vel = (p2.velocities[0] + p3.velocities[0]) / 2; + EXPECT_NEAR(half_p2_to_p3_vel, expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_third_seg - velocity_second_seg / (time_third_seg - time_second_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 3s after msg + double position_third_seg = position_second_seg + (p2.velocities[0] + p3.velocities[0]) / 2 * + (time_third_seg - time_second_seg); + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); + // the goal is reached so no acceleration anymore + EXPECT_NEAR(0, expected_state.accelerations[0], EPS); + } + + // sample past given points - movement virtually stops + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, + start, end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } +} + +// This test is added because previous one behaved strange if +// "point_before_msg.velocities.push_back(0.0);" was not defined +TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_without_vel) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double time_third_seg = 3.0; + double velocity_first_seg = 1.0; + double velocity_second_seg = 2.0; + double velocity_third_seg = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.velocities.push_back(velocity_first_seg); + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.velocities.push_back(velocity_second_seg); + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.velocities.push_back(velocity_third_seg); + p3.time_from_start = rclcpp::Duration::from_seconds(time_third_seg); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(0.0, expected_state.velocities[0], EPS); + // is 0 because point_before_msg does not have velocity defined + EXPECT_NEAR(1.0, expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + // double half_current_to_p1 = point_before_msg.positions[0] + + // (point_before_msg.velocities[0] + + // ((point_before_msg.velocities[0] + p1.velocities[0]) / 2 - + // point_before_msg.velocities[0]) / 2) * 0.5; + double half_current_to_p1 = + point_before_msg.positions[0] + (0.0 + ((0.0 + p1.velocities[0]) / 2 - 0.0) / 2) * 0.5; + EXPECT_NEAR(half_current_to_p1, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0] / 2, expected_state.velocities[0], EPS); + EXPECT_NEAR(1.0, expected_state.accelerations[0], EPS); + } + + // sample 1s after msg + double position_first_seg = + point_before_msg.positions[0] + (0.0 + p1.velocities[0]) / 2 * time_first_seg; + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(1.0, expected_state.accelerations[0], EPS); + } +} + +TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double time_third_seg = 3.0; + double acceleration_first_seg = 1.0; + double acceleration_second_seg = 2.0; + double acceleration_third_seg = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.accelerations.push_back(acceleration_first_seg); + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.accelerations.push_back(acceleration_second_seg); + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.accelerations.push_back(acceleration_third_seg); + p3.time_from_start = rclcpp::Duration::from_seconds(time_third_seg); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + point_before_msg.velocities.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(0.0, expected_state.velocities[0], EPS); + // is 0 because point_before_msg does not have velocity defined + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(result, false); + } + + // Sample only on points testing of intermediate values is too complex and not necessary + + // sample 1s after msg + double velocity_first_seg = + point_before_msg.velocities[0] + (0.0 + p1.accelerations[0]) / 2 * time_first_seg; + double position_first_seg = + point_before_msg.positions[0] + (0.0 + velocity_first_seg) / 2 * time_first_seg; + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_first_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p1.accelerations[0], expected_state.accelerations[0], EPS); + } + + // sample 2s after msg + double velocity_second_seg = velocity_first_seg + (p1.accelerations[0] + p2.accelerations[0]) / + 2 * (time_second_seg - time_first_seg); + double position_second_seg = position_first_seg + (velocity_first_seg + velocity_second_seg) / 2 * + (time_second_seg - time_first_seg); + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ((++traj.begin()), start); + EXPECT_EQ((--traj.end()), end); + EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_second_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p2.accelerations[0], expected_state.accelerations[0], EPS); + } + + // sample 3s after msg + double velocity_third_seg = velocity_second_seg + (p2.accelerations[0] + p3.accelerations[0]) / + 2 * (time_third_seg - time_second_seg); + double position_third_seg = position_second_seg + (velocity_second_seg + velocity_third_seg) / 2 * + (time_third_seg - time_second_seg); + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_third_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p3.accelerations[0], expected_state.accelerations[0], EPS); + } + + // sample past given points - movement virtually stops + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, + start, end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_third_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p3.accelerations[0], expected_state.accelerations[0], EPS); + } +} + +TEST(TestTrajectory, skip_interpolation) +{ + // Simple passthrough without extra interpolation + { + const InterpolationMethod no_interpolation = InterpolationMethod::NONE; + + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, no_interpolation, expected_state, start, end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + // There were no vels/accels in the input, so they should remain empty + EXPECT_EQ( + static_cast::type>(0), expected_state.velocities.size()); + EXPECT_EQ( + static_cast::type>(0), expected_state.accelerations.size()); + } + + // sample before time_now + { + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, + end); + ASSERT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + // For passthrough, this should just return the first waypoint + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + // There were no vels/accels in the input, so they should remain empty + EXPECT_EQ( + static_cast::type>(0), expected_state.velocities.size()); + EXPECT_EQ( + static_cast::type>(0), expected_state.accelerations.size()); + } + + // sample 1s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); + // There were no vels/accels in the input, so they should remain empty + EXPECT_EQ( + static_cast::type>(0), expected_state.velocities.size()); + EXPECT_EQ( + static_cast::type>(0), expected_state.accelerations.size()); + } + + // sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); + } + + // sample 2.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, + end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample 3s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, + end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample past given points + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, + end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + } +} diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp new file mode 100644 index 00000000..10dd3a6b --- /dev/null +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -0,0 +1,571 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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 _MSC_VER +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "action_msgs/msg/goal_status_array.hpp" +#include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gtest/gtest.h" +#include "hardware_interface/resource_manager.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "test_trajectory_controller_utils.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; +using test_trajectory_controllers::TestableJointTrajectoryController; +using test_trajectory_controllers::TrajectoryControllerTest; +using trajectory_msgs::msg::JointTrajectoryPoint; + +class TestTrajectoryActions : public TrajectoryControllerTest +{ +protected: + void SetUp() + { + TrajectoryControllerTest::SetUp(); + goal_options_.result_callback = + std::bind(&TestTrajectoryActions::common_result_response, this, _1); + goal_options_.feedback_callback = nullptr; + } + + void SetUpExecutor(const std::vector & parameters = {}) + { + setup_executor_ = true; + + executor_ = std::make_unique(); + + SetUpAndActivateTrajectoryController(true, parameters); + + executor_->add_node(traj_controller_->get_node()->get_node_base_interface()); + + SetUpActionClient(); + + executor_->add_node(node_->get_node_base_interface()); + + executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_->spin(); }); + } + + void SetUpControllerHardware() + { + setup_controller_hw_ = true; + + controller_hw_thread_ = std::thread( + [&]() + { + // controller hardware cycle update loop + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + auto start_time = clock.now(); + rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); + auto end_time = start_time + wait; + while (clock.now() < end_time) + { + traj_controller_->update(clock.now(), clock.now() - start_time); + } + }); + + // sometimes doesn't receive calls when we don't sleep + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + } + + void SetUpActionClient() + { + action_client_ = rclcpp_action::create_client( + node_->get_node_base_interface(), node_->get_node_graph_interface(), + node_->get_node_logging_interface(), node_->get_node_waitables_interface(), + controller_name_ + "/follow_joint_trajectory"); + + bool response = action_client_->wait_for_action_server(std::chrono::seconds(1)); + if (!response) + { + throw std::runtime_error("could not get action server"); + } + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() + { + TearDownControllerHardware(); + TearDownExecutor(); + } + + void TearDownExecutor() + { + if (setup_executor_) + { + setup_executor_ = false; + executor_->cancel(); + executor_future_handle_.wait(); + } + } + + void TearDownControllerHardware() + { + if (setup_controller_hw_) + { + setup_controller_hw_ = false; + if (controller_hw_thread_.joinable()) + { + controller_hw_thread_.join(); + } + } + } + + using FollowJointTrajectoryMsg = control_msgs::action::FollowJointTrajectory; + using GoalHandle = rclcpp_action::ClientGoalHandle; + using GoalOptions = rclcpp_action::Client::SendGoalOptions; + + std::shared_future sendActionGoal( + const std::vector & points, double timeout, const GoalOptions & opt) + { + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return action_client_->async_send_goal(goal_msg, opt); + } + + rclcpp_action::Client::SharedPtr action_client_; + rclcpp_action::ResultCode common_resultcode_ = rclcpp_action::ResultCode::UNKNOWN; + int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; + + bool setup_executor_ = false; + rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_; + std::future executor_future_handle_; + + bool setup_controller_hw_ = false; + std::thread controller_hw_thread_; + + GoalOptions goal_options_; + +public: + void common_result_response(const GoalHandle::WrappedResult & result) + { + RCLCPP_DEBUG( + node_->get_logger(), "common_result_response time: %f", rclcpp::Clock().now().seconds()); + common_resultcode_ = result.code; + common_action_result_code_ = result.result->error_code; + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_DEBUG(node_->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_DEBUG(node_->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_DEBUG(node_->get_logger(), "Unknown result code"); + return; + } + } +}; + +TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) +{ + SetUpExecutor(); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + EXPECT_EQ(1.0, joint_pos_[0]); + EXPECT_EQ(2.0, joint_pos_[1]); + EXPECT_EQ(3.0, joint_pos_[2]); +} + +TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) +{ + SetUpExecutor(); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_state_tolerances_fail) +{ + // set joint tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, + common_action_result_code_); + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) +{ + // set joint 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.joint1.goal", goal_tol), + rclcpp::Parameter("constraints.joint2.goal", goal_tol), + rclcpp::Parameter("constraints.joint3.goal", goal_tol), + rclcpp::Parameter("constraints.goal_time", goal_time)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + const double init_pos1 = joint_pos_[0]; + const double init_pos2 = joint_pos_[1]; + const double init_pos3 = joint_pos_[2]; + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 4.0; + point.positions[1] = 5.0; + point.positions[2] = 6.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, + common_action_result_code_); + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) +{ + // set joint tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + const double init_pos1 = joint_pos_[0]; + const double init_pos2 = joint_pos_[1]; + const double init_pos3 = joint_pos_[2]; + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 4.0; + point.positions[1] = 5.0; + point.positions[2] = 6.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, + common_action_result_code_); + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_cancel_hold_position) +{ + SetUpExecutor(); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(1.0); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 4.0; + point.positions[1] = 5.0; + point.positions[2] = 6.0; + points.push_back(point); + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(2.0); + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + // send and wait for half a second before cancel + gh_future = action_client_->async_send_goal(goal_msg, goal_options_); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + const auto goal_handle = gh_future.get(); + action_client_->async_cancel_goal(goal_handle); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + const double prev_pos1 = joint_pos_[0]; + const double prev_pos2 = joint_pos_[1]; + const double prev_pos3 = joint_pos_[2]; + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_EQ(prev_pos1, joint_pos_[0]); + EXPECT_EQ(prev_pos2, joint_pos_[1]); + EXPECT_EQ(prev_pos3, joint_pos_[2]); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp new file mode 100644 index 00000000..2a1b526f --- /dev/null +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -0,0 +1,1641 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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 "gmock/gmock.h" + +#include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp" +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_event.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/header.hpp" +#include "test_trajectory_controller_utils.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +using lifecycle_msgs::msg::State; +using test_trajectory_controllers::TrajectoryControllerTest; +using test_trajectory_controllers::TrajectoryControllerTestParameterized; + +bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } + +void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } + +TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + // const auto future_handle_ = std::async(std::launch::async, spin, &executor); + + 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_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + 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, joint_pos_[0]); + EXPECT_EQ(0.0, joint_pos_[1]); + EXPECT_EQ(0.0, joint_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); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + 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(), joint_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(joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + 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(), joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + +TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + // set command_joints parameter + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); + traj_controller_->get_node()->set_parameter({command_joint_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + 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(), joint_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_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + 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_joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + +TEST_P(TrajectoryControllerTestParameterized, activate) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + traj_controller_->get_node()->configure(); + ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + + auto cmd_interface_config = traj_controller_->command_interface_configuration(); + ASSERT_EQ( + cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); + + auto state_interface_config = traj_controller_->state_interface_configuration(); + ASSERT_EQ( + state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); + + ActivateTrajectoryController(); + ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + + executor.cancel(); +} + +// TEST_F(TestTrajectoryController, activation) { +// auto traj_controller = std::make_shared( +// joint_names_, op_mode_); +// auto ret = traj_controller->init(test_robot_, controller_name_); +// if (ret != controller_interface::return_type::OK) { +// FAIL(); +// } +// +// auto traj_node = traj_controller->get_node(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(traj_node->get_node_base_interface()); +// +// auto state = traj_controller_->configure(); +// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); +// +// state = traj_node->activate(); +// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); +// +// // wait for the subscriber and publisher to completely setup +// std::this_thread::sleep_for(std::chrono::seconds(2)); +// +// // send msg +// builtin_interfaces::msg::Duration time_from_start; +// time_from_start.sec = 1; +// time_from_start.nanosec = 0; +// std::vector> points {{{3.3, 4.4, 5.5}}}; +// publish(time_from_start, points, rclcpp::Time()); +// // wait for msg is be published to the system +// std::this_thread::sleep_for(std::chrono::milliseconds(1000)); +// executor.spin_once(); +// +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// resource_manager_->write(); +// +// // change in hw position +// EXPECT_EQ(3.3, joint_pos_[0]); +// EXPECT_EQ(4.4, joint_pos_[1]); +// EXPECT_EQ(5.5, joint_pos_[2]); +// +// executor.cancel(); +// } + +// TEST_F(TestTrajectoryController, reactivation) { +// auto traj_controller = std::make_shared( +// joint_names_, op_mode_); +// auto ret = traj_controller->init(test_robot_, controller_name_); +// if (ret != controller_interface::return_type::OK) { +// FAIL(); +// } +// +// auto traj_node = traj_controller->get_node(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(traj_node->get_node_base_interface()); +// +// auto state = traj_controller_->configure(); +// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); +// +// state = traj_node->activate(); +// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); +// +// // wait for the subscriber and publisher to completely setup +// std::this_thread::sleep_for(std::chrono::seconds(2)); +// +// // send msg +// builtin_interfaces::msg::Duration time_from_start; +// time_from_start.sec = 1; +// time_from_start.nanosec = 0; +// // *INDENT-OFF* +// std::vector> points { +// {{3.3, 4.4, 5.5}}, +// {{7.7, 8.8, 9.9}}, +// {{10.10, 11.11, 12.12}} +// }; +// // *INDENT-ON* +// publish(time_from_start, points, rclcpp::Time()); +// // wait for msg is be published to the system +// std::this_thread::sleep_for(std::chrono::milliseconds(500)); +// executor.spin_once(); +// +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// resource_manager_->write(); +// +// // deactivated +// // wait so controller process the second point when deactivated +// std::this_thread::sleep_for(std::chrono::milliseconds(500)); +// state = traj_controller_->deactivate(); +// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); +// resource_manager_->read(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// resource_manager_->write(); +// +// // no change in hw position +// EXPECT_EQ(3.3, joint_pos_[0]); +// EXPECT_EQ(4.4, joint_pos_[1]); +// EXPECT_EQ(5.5, joint_pos_[2]); +// +// // reactivated +// // wait so controller process the third point when reactivated +// std::this_thread::sleep_for(std::chrono::milliseconds(3000)); +// state = traj_node->activate(); +// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); +// resource_manager_->read(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// resource_manager_->write(); +// +// // change in hw position to 3rd point +// EXPECT_EQ(10.10, joint_pos_[0]); +// EXPECT_EQ(11.11, joint_pos_[1]); +// EXPECT_EQ(12.12, joint_pos_[2]); +// +// executor.cancel(); +// } + +TEST_P(TrajectoryControllerTestParameterized, cleanup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor); + + // 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_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + 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()); + // update for 0.25 seconds + updateController(rclcpp::Duration::from_seconds(0.25)); + + // should be home pose again + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + + 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, false); + + // This call is replacing the way parameters are set via launch + SetParameters(); + traj_controller_->configure(); + auto state = traj_controller_->get_state(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + ActivateTrajectoryController(); + + state = traj_controller_->get_state(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); + EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); + EXPECT_EQ(INITIAL_POS_JOINT3, joint_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_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + + // wait so controller process the second point when deactivated + traj_controller_->update( + rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); + // deactivated + state = traj_controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + const auto allowed_delta = 0.05; + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + // cleanup + state = traj_controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // TODO(anyone): should the controller even allow calling update() when it is not active? + // update loop receives a new msg and updates accordingly + traj_controller_->update( + rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); + + if (traj_controller_->has_position_command_interface()) + { + // otherwise this won't be updated + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } + + // state = traj_controller_->get_node()->configure(); + // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +{ + // Skip for now, test is unstable + GTEST_SKIP() << "Skipping position_error_not_normalized test"; + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}); + subscribeToState(); + updateController(); + + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + + size_t n_joints = joint_names_.size(); + + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_EQ(joint_names_[i], state->joint_names[i]); + } + + // No trajectory by default, no desired state or error + EXPECT_TRUE(state->desired.positions.empty() || state->desired.positions == INITIAL_POS_JOINTS); + EXPECT_TRUE(state->desired.velocities.empty() || state->desired.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); + + EXPECT_EQ(n_joints, state->actual.positions.size()); + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->actual.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->actual.velocities.size()); + } + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->actual.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->actual.accelerations.size()); + } + + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); +} + +// Floating-point value comparison threshold +const double EPS = 1e-6; +/** + * @brief check if position error of revolute joints are normalized if not configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +{ + // Skip for now, test is unstable + GTEST_SKIP() << "Skipping position_error_not_normalized test"; + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // no normalization of position error + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * allowed_delta); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are normalized if configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +{ + // Skip for now, test is unstable + GTEST_SKIP() << "Skipping position_error_normalized test"; + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // is error.positions[2] normalized? + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], + state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_GE(0.0, joint_vel_[2]); + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // is error of positions[2] normalized? + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + EXPECT_LE(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + +// /** +// * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from +// * internal controller order +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// { +// trajectory_msgs::msg::JointTrajectory traj_msg; +// const std::vector jumbled_joint_names{ +// joint_names_[1], joint_names_[2], joint_names_[0]}; +// traj_msg.joint_names = jumbled_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(3); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 3.0; +// traj_msg.points[0].positions[2] = 1.0; + +// if (traj_controller_->has_velocity_command_interface()) +// { +// traj_msg.points[0].velocities.resize(3); +// traj_msg.points[0].velocities[0] = -0.1; +// traj_msg.points[0].velocities[1] = -0.1; +// traj_msg.points[0].velocities[2] = -0.1; +// } + +// if (traj_controller_->has_effort_command_interface()) +// { +// traj_msg.points[0].effort.resize(3); +// traj_msg.points[0].effort[0] = -0.1; +// traj_msg.points[0].effort[1] = -0.1; +// traj_msg.points[0].effort[2] = -0.1; +// } + +// trajectory_publisher_->publish(traj_msg); +// } + +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.25 seconds +// // TODO(destogl): Make this time a bit shorter to increase stability on the CI? +// // Currently COMMON_THRESHOLD is adjusted. +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// if (traj_controller_->has_position_command_interface()) +// { +// EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); +// EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); +// } + +// if (traj_controller_->has_velocity_command_interface()) +// { +// EXPECT_GT(0.0, joint_vel_[0]); +// EXPECT_GT(0.0, joint_vel_[1]); +// EXPECT_GT(0.0, joint_vel_[2]); +// } + +// if (traj_controller_->has_effort_command_interface()) +// { +// EXPECT_GT(0.0, joint_eff_[0]); +// EXPECT_GT(0.0, joint_eff_[1]); +// EXPECT_GT(0.0, joint_eff_[2]); +// } +// } + +// /** +// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled +// * joints +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// const double initial_joint1_cmd = joint_pos_[0]; +// const double initial_joint2_cmd = joint_pos_[1]; +// const double initial_joint3_cmd = joint_pos_[2]; +// trajectory_msgs::msg::JointTrajectory traj_msg; + +// { +// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; +// traj_msg.joint_names = partial_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(2); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 1.0; +// traj_msg.points[0].velocities.resize(2); +// traj_msg.points[0].velocities[0] = +// copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); +// traj_msg.points[0].velocities[1] = +// copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + +// trajectory_publisher_->publish(traj_msg); +// } + +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.5 seconds +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// double threshold = 0.001; + +// if (traj_controller_->has_position_command_interface()) +// { +// EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); +// EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); +// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) +// << "Joint 3 command should be current position"; +// } + +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != +// command_interface_types_.end()) +// { +// // estimate the sign of the velocity +// // joint rotates forward +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); +// EXPECT_NEAR(0.0, joint_vel_[2], threshold) +// << "Joint 3 velocity should be 0.0 since it's not in the goal"; +// } + +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != +// command_interface_types_.end()) +// { +// // estimate the sign of the velocity +// // joint rotates forward +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); +// EXPECT_NEAR(0.0, joint_eff_[2], threshold) +// << "Joint 3 effort should be 0.0 since it's not in the goal"; +// } +// // TODO(anyone): add here ckecks for acceleration commands + +// executor.cancel(); +// } + +// /** +// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled +// * joints without allow_partial_joints_goal +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// const double initial_joint1_cmd = joint_pos_[0]; +// const double initial_joint2_cmd = joint_pos_[1]; +// const double initial_joint3_cmd = joint_pos_[2]; +// const double initial_joint_vel = 0.0; +// const double initial_joint_acc = 0.0; +// trajectory_msgs::msg::JointTrajectory traj_msg; + +// { +// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; +// traj_msg.joint_names = partial_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(2); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 1.0; +// traj_msg.points[0].velocities.resize(2); +// traj_msg.points[0].velocities[0] = 2.0; +// traj_msg.points[0].velocities[1] = 1.0; + +// trajectory_publisher_->publish(traj_msg); +// } + +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.5 seconds +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// double threshold = 0.001; +// EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) +// << "All joints command should be current position because goal was rejected"; +// EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) +// << "All joints command should be current position because goal was rejected"; +// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) +// << "All joints command should be current position because goal was rejected"; + +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != +// command_interface_types_.end()) +// { +// EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// } + +// if ( +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != +// command_interface_types_.end()) +// { +// EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// } + +// executor.cancel(); +// } + +// /** +// * @brief invalid_message Test mismatched joint and reference vector sizes +// */ +// TEST_P(TrajectoryControllerTestParameterized, invalid_message) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +// rclcpp::Parameter allow_integration_parameters( +// "allow_integration_in_goal_trajectories", false); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController( +// true, {partial_joints_parameters, allow_integration_parameters}, &executor); + +// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + +// good_traj_msg.joint_names = joint_names_; +// good_traj_msg.header.stamp = rclcpp::Time(0); +// good_traj_msg.points.resize(1); +// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// good_traj_msg.points[0].positions.resize(1); +// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; +// good_traj_msg.points[0].velocities.resize(1); +// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + +// // Incompatible joint names +// traj_msg = good_traj_msg; +// traj_msg.joint_names = {"bad_name"}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // No position data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too many positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few velocities +// traj_msg = good_traj_msg; +// traj_msg.points[0].velocities = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few accelerations +// traj_msg = good_traj_msg; +// traj_msg.points[0].accelerations = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few efforts +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].effort = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Non-strictly increasing waypoint times +// traj_msg = good_traj_msg; +// traj_msg.points.push_back(traj_msg.points.front()); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +// } + +// /// With allow_integration_in_goal_trajectories parameter trajectory missing position or +// /// velocities are accepted +// TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) +// { +// rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); + +// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + +// good_traj_msg.joint_names = joint_names_; +// good_traj_msg.header.stamp = rclcpp::Time(0); +// good_traj_msg.points.resize(1); +// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// good_traj_msg.points[0].positions.resize(1); +// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; +// good_traj_msg.points[0].velocities.resize(1); +// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; +// good_traj_msg.points[0].accelerations.resize(1); +// good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + +// // No position data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // No position and velocity data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].velocities.clear(); +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // All empty +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].velocities.clear(); +// traj_msg.points[0].accelerations.clear(); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too many positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few velocities +// traj_msg = good_traj_msg; +// traj_msg.points[0].velocities = {1.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few accelerations +// traj_msg = good_traj_msg; +// traj_msg.points[0].accelerations = {2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +// } + +// /** +// * @brief test_trajectory_replace Test replacing an existing trajectory +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// subscribeToState(); + +// std::vector> points_old{{{2., 3., 4.}}}; +// std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; +// std::vector> points_partial_new{{1.5}}; +// std::vector> points_partial_new_velocities{{0.15}}; + +// 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(), {}, points_old_velocities); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_actual.velocities = { +// points_old_velocities[0].begin(), points_old_velocities[0].end()}; +// expected_desired.velocities = { +// points_old_velocities[0].begin(), points_old_velocities[0].end()}; +// // Check that we reached end of points_old trajectory +// // Denis: delta was 0.1 with 0.2 works for me +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + +// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); +// points_partial_new_velocities[0][0] = +// copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); +// publish( +// time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + +// // Replaced trajectory is a mix of previous and current goal +// expected_desired.positions[0] = points_partial_new[0][0]; +// expected_desired.positions[1] = points_old[0][1]; +// expected_desired.positions[2] = points_old[0][2]; +// expected_desired.velocities[0] = points_partial_new_velocities[0][0]; +// expected_desired.velocities[1] = 0.0; +// expected_desired.velocities[2] = 0.0; +// expected_actual = expected_desired; +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } + +// /** +// * @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(true, {}, &executor); +// subscribeToState(); + +// // TODO(anyone): add expectations for velocities and accelerations +// 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()); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0] trajectory +// waitAndCompareState(expected_actual, expected_desired, 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().now() - delay - std::chrono::milliseconds(100); +// expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; +// expected_desired = expected_actual; +// std::cout << "Sending old trajectory" << std::endl; +// publish(time_from_start, points_new, new_traj_start); +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } + +// TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); + +// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; +// std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + +// 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()); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0]trajectory +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// 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 = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); +// expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; +// expected_desired = expected_actual; +// publish(time_from_start, points_new, new_traj_start); +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } + +// TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +// { +// SetUpTrajectoryController(); +// auto traj_node = traj_controller_->get_node(); +// RCLCPP_WARN( +// traj_node->get_logger(), +// "Test disabled until current_trajectory is taken into account when adding a new trajectory."); +// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ +// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 +// return; + +// // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line +// rclcpp::executors::SingleThreadedExecutor executor; +// executor.add_node(traj_node->get_node_base_interface()); +// subscribeToState(); +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); +// traj_node->set_parameter(partial_joints_parameters); +// traj_controller_->get_node()->configure(); +// traj_controller_->get_node()->activate(); + +// std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; +// std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; +// std::vector> partial_traj{ +// {{-1., -2.}, +// { +// -2., +// -4, +// }}}; +// std::vector> partial_traj_velocities{ +// {{-0.1, -0.2}, +// { +// -0.2, +// -0.4, +// }}}; +// 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_velocities); +// // Sleep until first waypoint of full trajectory + +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0]trajectory and are starting points_old[1] +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// // Send partial trajectory starting after full trajecotry is complete +// RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); +// publish( +// points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); +// // Wait until the end start and end of partial traj + +// expected_actual.positions = { +// partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; +// expected_desired = expected_actual; + +// waitAndCompareState( +// expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); +// } + +// 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(true, {is_open_loop_parameters}, &executor, true); + +// // goal setup +// std::vector first_goal = {3.3, 4.4, 5.5}; +// std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; +// std::vector second_goal = {6.6, 8.8, 11.0}; +// std::vector> second_goal_velocities = {{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; +// std::vector> points{{first_goal}}; +// publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); +// traj_controller_->wait_for_trajectory(executor); +// updateController(rclcpp::Duration::from_seconds(1.1)); + +// if (traj_controller_->has_position_command_interface()) +// { +// // 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_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + +// // Move joint further in the same direction as before (to the second goal) +// points = {{second_goal}}; +// publish(time_from_start, points, rclcpp::Time(1.0), {}, second_goal_velocities); +// 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], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // Expect backward commands at first +// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); + +// // Finally the second goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + +// // Move joint back to the first goal +// points = {{first_goal}}; +// publish(time_from_start, points, rclcpp::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], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // Expect backward commands at first +// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); + +// // Finally the first goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// } + +// executor.cancel(); +// } + +// TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_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", true); +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + +// // 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; +// std::vector> points{{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); +// updateController(rclcpp::Duration::from_seconds(1.1)); + +// if (traj_controller_->has_position_command_interface()) +// { +// // 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_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + +// // Move joint further in the same direction as before (to the second goal) +// points = {{second_goal}}; +// publish(time_from_start, points, rclcpp::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], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // There should not be backward commands +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); + +// // Finally the second goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + +// // Move joint back to the first goal +// points = {{first_goal}}; +// publish(time_from_start, points, rclcpp::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], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // There should not be a jump toward commands +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); + +// // Finally the first goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(first_goal[0], joint_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 state interfaces +// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + +// // set command values to NaN +// for (size_t i = 0; i < 3; ++i) +// { +// joint_pos_[i] = std::numeric_limits::quiet_NaN(); +// joint_vel_[i] = std::numeric_limits::quiet_NaN(); +// joint_acc_[i] = std::numeric_limits::quiet_NaN(); +// } +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + +// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + +// for (size_t i = 0; i < 3; ++i) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + +// // check velocity +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); +// } + +// // check acceleration +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); +// } +// } + +// executor.cancel(); +// } + +// // Testing that values are read from state 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; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + +// // set command values to NaN +// for (size_t i = 0; i < 3; ++i) +// { +// joint_pos_[i] = 3.1 + i; +// joint_vel_[i] = 0.25 + i; +// joint_acc_[i] = 0.02 + i / 10.0; +// } +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + +// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + +// for (size_t i = 0; i < 3; ++i) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + +// // check velocity +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); +// } + +// // check acceleration +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); +// } +// } + +// executor.cancel(); +// } + +// 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"})))); + +// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` +// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +// { +// auto set_parameter_and_check_result = [&]() +// { +// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +// SetParameters(); // This call is replacing the way parameters are set via launch +// traj_controller_->get_node()->configure(); +// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +// }; +// +// SetUpTrajectoryController(false); +// +// // command interfaces: empty +// command_interface_types_ = {}; +// set_parameter_and_check_result(); +// +// // command interfaces: bad_name +// command_interface_types_ = {"bad_name"}; +// set_parameter_and_check_result(); +// +// // command interfaces: effort has to be only +// command_interface_types_ = {"effort", "position"}; +// set_parameter_and_check_result(); +// +// // command interfaces: velocity - position not present +// command_interface_types_ = {"velocity", "acceleration"}; +// set_parameter_and_check_result(); +// +// // command interfaces: acceleration without position and velocity +// command_interface_types_ = {"acceleration"}; +// set_parameter_and_check_result(); +// +// // state interfaces: empty +// state_interface_types_ = {}; +// set_parameter_and_check_result(); +// +// // state interfaces: cannot not be effort +// state_interface_types_ = {"effort"}; +// set_parameter_and_check_result(); +// +// // state interfaces: bad name +// state_interface_types_ = {"bad_name"}; +// set_parameter_and_check_result(); +// +// // state interfaces: velocity - position not present +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity", "acceleration"}; +// set_parameter_and_check_result(); +// +// // state interfaces: acceleration without position and velocity +// state_interface_types_ = {"acceleration"}; +// set_parameter_and_check_result(); +// +// // velocity-only command interface: position - velocity not present +// command_interface_types_ = {"velocity"}; +// state_interface_types_ = {"position"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// +// // effort-only command interface: position - velocity not present +// command_interface_types_ = {"effort"}; +// state_interface_types_ = {"position"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp new file mode 100644 index 00000000..25612f56 --- /dev/null +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -0,0 +1,463 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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_TRAJECTORY_CONTROLLER_UTILS_HPP_ +#define TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" + +namespace +{ +const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double INITIAL_POS_JOINT1 = 1.1; +const double INITIAL_POS_JOINT2 = 2.1; +const double INITIAL_POS_JOINT3 = 3.1; +const std::vector INITIAL_POS_JOINTS = { + INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; +const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; +const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; +const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +} // namespace + +namespace test_trajectory_controllers +{ +class TestableJointTrajectoryController +: public joint_trajectory_controller::JointTrajectoryController +{ +public: + using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController; + using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); + // this class can still be useful without the wait set + if (joint_command_subscriber_) + { + joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); + } + return ret; + } + + /** + * @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 + * + * @return true if new JointTrajectory msg was received, false if timeout + */ + bool wait_for_trajectory( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + void set_joint_names(const std::vector & joint_names) + { + params_.joints = joint_names; + } + + void set_command_joint_names(const std::vector & command_joint_names) + { + command_joint_names_ = command_joint_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(); } + + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() + { + return last_commanded_state_; + } + + bool has_position_command_interface() { return has_position_command_interface_; } + + bool has_velocity_command_interface() { return has_velocity_command_interface_; } + + bool has_effort_command_interface() { return has_effort_command_interface_; } + + bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + + rclcpp::WaitSet joint_cmd_sub_wait_set_; +}; + +class TrajectoryControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + virtual void SetUp() + { + controller_name_ = "test_joint_trajectory_controller"; + + joint_names_ = {"joint1", "joint2", "joint3"}; + command_joint_names_ = { + "following_controller/joint1", "following_controller/joint2", "following_controller/joint3"}; + joint_pos_.resize(joint_names_.size(), 0.0); + joint_state_pos_.resize(joint_names_.size(), 0.0); + joint_vel_.resize(joint_names_.size(), 0.0); + joint_state_vel_.resize(joint_names_.size(), 0.0); + joint_acc_.resize(joint_names_.size(), 0.0); + joint_state_acc_.resize(joint_names_.size(), 0.0); + joint_eff_.resize(joint_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_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); + } + + void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true) + { + traj_controller_ = std::make_shared(); + + if (use_local_parameters) + { + traj_controller_->set_joint_names(joint_names_); + traj_controller_->set_command_interfaces(command_interface_types_); + traj_controller_->set_state_interfaces(state_interface_types_); + } + auto ret = traj_controller_->init(controller_name_); + if (ret != controller_interface::return_type::OK) + { + FAIL(); + } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetParameters(); + } + + void SetParameters() + { + auto node = traj_controller_->get_node(); + const rclcpp::Parameter joint_names_param("joints", joint_names_); + const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_); + const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); + node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); + } + + void SetPidParameters( + double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) + { + traj_controller_->trigger_declare_parameters(); + auto node = traj_controller_->get_node(); + + for (size_t i = 0; i < joint_names_.size(); ++i) + { + const std::string prefix = "gains." + joint_names_[i]; + const rclcpp::Parameter k_p(prefix + ".p", p_default); + 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_default); + const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); + } + } + + void SetUpAndActivateTrajectoryController( + rclcpp::Executor & executor, bool use_local_parameters = true, + const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + bool normalize_error = false) + { + SetUpTrajectoryController(executor, use_local_parameters); + + // set pid parameters before configure + SetPidParameters(k_p, ff, normalize_error); + for (const auto & param : parameters) + { + traj_controller_->get_node()->set_parameter(param); + } + // ignore velocity tolerances for this test since they aren't committed in test_robot->write() + rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); + traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); + + traj_controller_->get_node()->configure(); + + ActivateTrajectoryController(separate_cmd_and_state_values); + } + + void ActivateTrajectoryController(bool separate_cmd_and_state_values = false) + { + std::vector cmd_interfaces; + std::vector state_interfaces; + pos_cmd_interfaces_.reserve(joint_names_.size()); + vel_cmd_interfaces_.reserve(joint_names_.size()); + acc_cmd_interfaces_.reserve(joint_names_.size()); + eff_cmd_interfaces_.reserve(joint_names_.size()); + pos_state_interfaces_.reserve(joint_names_.size()); + vel_state_interfaces_.reserve(joint_names_.size()); + acc_state_interfaces_.reserve(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); + vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); + acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); + eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); + + pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, + separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); + vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, + separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); + acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_ACCELERATION, + separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); + + // Add to export lists and set initial values + cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); + cmd_interfaces.back().set_value(INITIAL_POS_JOINTS[i]); + cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); + cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); + cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); + cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]); + cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); + cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]); + joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; + joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; + joint_state_acc_[i] = INITIAL_ACC_JOINTS[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)); + traj_controller_->get_node()->activate(); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void subscribeToState() + { + auto traj_lifecycle_node = traj_controller_->get_node(); + + using control_msgs::msg::JointTrajectoryControllerState; + + 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_ + "/state", qos, + [&](std::shared_ptr msg) + { + std::lock_guard guard(state_mutex_); + state_msg_ = msg; + }); + } + + /// Publish trajectory msgs with multiple points + /** + * delay_btwn_points - delay between each points + * points - vector of trajectories. One point per controlled joint + * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided points + */ + void publish( + const builtin_interfaces::msg::Duration & delay_btwn_points, + const std::vector> & points, rclcpp::Time start_time, + const std::vector & joint_names = {}, + const std::vector> & points_velocities = {}) + { + 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; + } + + trajectory_msgs::msg::JointTrajectory traj_msg; + if (joint_names.empty()) + { + traj_msg.joint_names = {joint_names_.begin(), joint_names_.begin() + points[0].size()}; + } + else + { + traj_msg.joint_names = joint_names; + } + traj_msg.header.stamp = start_time; + traj_msg.points.resize(points.size()); + + 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.size(); ++index) + { + traj_msg.points[index].time_from_start = duration_total; + + traj_msg.points[index].positions.resize(points[index].size()); + for (size_t j = 0; j < points[index].size(); ++j) + { + traj_msg.points[index].positions[j] = points[index][j]; + } + duration_total = duration_total + duration; + } + + for (size_t index = 0; index < points_velocities.size(); ++index) + { + traj_msg.points[index].velocities.resize(points_velocities[index].size()); + for (size_t j = 0; j < points_velocities[index].size(); ++j) + { + traj_msg.points[index].velocities[j] = points_velocities[index][j]; + } + } + + trajectory_publisher_->publish(traj_msg); + } + + void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + { + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const auto end_time = start_time + wait_time; + while (clock.now() < end_time) + { + traj_controller_->update(clock.now(), clock.now() - start_time); + } + } + + void waitAndCompareState( + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, + trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, + rclcpp::Duration controller_wait_time, double allowed_delta) + { + { + std::lock_guard guard(state_mutex_); + state_msg_.reset(); + } + traj_controller_->wait_for_trajectory(executor); + updateController(controller_wait_time); + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + SCOPED_TRACE("Joint " + std::to_string(i)); + // TODO(anyone): add checking for velocties and accelerations + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + } + } + + for (size_t i = 0; i < expected_desired.positions.size(); ++i) + { + SCOPED_TRACE("Joint " + std::to_string(i)); + // TODO(anyone): add checking for velocties and accelerations + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + } + } + } + + std::shared_ptr getState() const + { + std::lock_guard guard(state_mutex_); + return state_msg_; + } + + std::string controller_name_; + + std::vector joint_names_; + std::vector command_joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr trajectory_publisher_; + + std::shared_ptr traj_controller_; + rclcpp::Subscription::SharedPtr + state_subscriber_; + mutable std::mutex state_mutex_; + std::shared_ptr state_msg_; + + std::vector joint_pos_; + std::vector joint_vel_; + std::vector joint_acc_; + std::vector joint_eff_; + std::vector joint_state_pos_; + std::vector joint_state_vel_; + std::vector joint_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_trajectory_controllers + +#endif // TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_