diff --git a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md b/.github/pull_request_template.md similarity index 92% rename from .github/PULL_REQUEST_TEMPLATE/pull_request_template.md rename to .github/pull_request_template.md index 87f389a124..8e9178c167 100644 --- a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,13 +1,3 @@ - ---- -name: Pull request -about: Create a pull request -title: '' -labels: '' -assignees: '' - ---- - Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: 1. Limited scope. Your PR should do one thing or one set of things. Avoid adding “random fixes” to PRs. Put those on separate PRs. diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 50c707e12b..84b156f5a1 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -16,13 +16,16 @@ groups: - rosterloh - progtologist - arne48 + - christophfroehlich - DasRoteSkelett - - Serafadam + - sgmurray - harderthan - jaron-l - malapatiravi - - homalozoa - erickisos + - sachinkum0009 + - qiayuanliao + - homalozoa - anfemosa - jackcenter - VX792 @@ -31,6 +34,7 @@ groups: - aprotyas - peterdavidfagan - duringhof + - VanshGehlot - bijoua29 - - lm2292 + - LukasMacha97 - mcbed diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 043f09dce3..6e6e96f032 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.2 + - uses: codecov/codecov-action@v3.1.3 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 9816e137c6..f0993bf717 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.5.0 + - uses: actions/setup-python@v4.6.0 with: python-version: '3.10' - name: Install system hooks diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 5dd0b6f086..fd3e6b634b 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -41,8 +41,8 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 - - uses: ros-tooling/action-ros-lint@v0.1 + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master with: distribution: rolling linter: cpplint diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 60837457f9..ed2da84ad5 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -.. _joint_trajectory_controller_userdoc: +.. _addmittance_controller_userdoc: Admittance Controller ====================== @@ -8,9 +8,6 @@ The controller implements ``ChainedControllerInterface``, so it is possible to a The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. -Parameters: - - ROS2 interface of the controller --------------------------------- diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 15d4bdd631..1c6b1dbd9e 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -41,13 +41,13 @@ Available Controllers :titlesonly: Admittance Controller <../admittance_controller/doc/userdoc.rst> - Tricycle Controller <../tricycle_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> - Effort Controllers <../effort_controllers/doc/userdoc.rst> Available Broadcasters diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 1f3a47997a..8292bdc1b3 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,9 +5,9 @@ forward_command_controller This is a base class implementing a feedforward controller. Specific implementations can be found in: -- :ref:`position_controllers_userdoc` -- :ref:`velocity_controllers_userdoc` -- :ref:`effort_controllers_userdoc` +* :ref:`position_controllers_userdoc` +* :ref:`velocity_controllers_userdoc` +* :ref:`effort_controllers_userdoc` Hardware interface type ----------------------- diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 8a030d9356..fbdb439992 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -21,26 +21,26 @@ If none of the requested interface are not defined, the controller returns error Parameters ---------- -``use_local_topics`` +use_local_topics Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. -``joints`` +joints Optional parameter (string array) to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``interfaces`` parameter. Joint state broadcaster asks for access to all defined interfaces on all defined joints. -``interfaces`` +interfaces Optional parameter (string array) to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``joints`` parameter. -``extra_joints`` +extra_joints Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. -``map_interface_to_joint_state`` +map_interface_to_joint_state Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. Usecases: diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index fc223d2eb0..1ba6257a5a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -10,11 +10,11 @@ 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. +* 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. +* 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. +* Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. Hardware interface type ----------------------- @@ -26,11 +26,11 @@ Similarly to the trajectory representation case above, it's possible to support Other features -------------- - Realtime-safe implementation. +* Realtime-safe implementation. - Proper handling of wrapping (continuous) joints. +* 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. +* Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. ros2_control interfaces ------------------------ @@ -45,10 +45,10 @@ The state interfaces are defined with ``joints`` and ``state_interfaces`` parame 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`` +* ``position`` +* ``position`` and ``velocity`` +* ``position``, ``velocity`` and ``acceleration`` +* ``effort`` Commands ^^^^^^^^^ @@ -230,7 +230,7 @@ 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] +~/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] @@ -244,23 +244,28 @@ The controller types are placed into namespaces according to their command types 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 +* 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 +.. * 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/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 2a6454f454..dfbdf7436e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -127,6 +127,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_current_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -256,6 +257,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void read_state_from_hardware(JointTrajectoryPoint & state); bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); void query_state_service( const std::shared_ptr request, @@ -267,6 +269,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + void resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5c719f7471..b6a60526fa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -467,6 +467,81 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } +bool JointTrajectoryController::read_commands_from_command_interfaces( + JointTrajectoryPoint & commands) +{ + 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 command. + if (has_position_command_interface_) + { + if (interface_has_values(joint_command_interface_[0])) + { + assign_point_from_interface(commands.positions, joint_command_interface_[0]); + } + else + { + commands.positions.clear(); + has_values = false; + } + } + if (has_velocity_command_interface_) + { + if (interface_has_values(joint_command_interface_[1])) + { + assign_point_from_interface(commands.velocities, joint_command_interface_[1]); + } + else + { + commands.velocities.clear(); + has_values = false; + } + } + if (has_acceleration_command_interface_) + { + if (interface_has_values(joint_command_interface_[2])) + { + assign_point_from_interface(commands.accelerations, joint_command_interface_[2]); + } + else + { + commands.accelerations.clear(); + has_values = false; + } + } + if (has_effort_command_interface_) + { + if (interface_has_values(joint_command_interface_[3])) + { + assign_point_from_interface(commands.effort, joint_command_interface_[3]); + } + else + { + commands.effort.clear(); + has_values = false; + } + } + + return has_values; +} + void JointTrajectoryController::query_state_service( const std::shared_ptr request, std::shared_ptr response) @@ -706,27 +781,44 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); - publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = get_node()->create_publisher( + "~/controller_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_.reference.positions.resize(dof_); + state_publisher_->msg_.reference.velocities.resize(dof_); + state_publisher_->msg_.reference.accelerations.resize(dof_); + state_publisher_->msg_.feedback.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_.feedback.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_.feedback.accelerations.resize(dof_); state_publisher_->msg_.error.accelerations.resize(dof_); } + if (has_position_command_interface_) + { + state_publisher_->msg_.output.positions.resize(dof_); + } + if (has_velocity_command_interface_) + { + state_publisher_->msg_.output.velocities.resize(dof_); + } + if (has_acceleration_command_interface_) + { + state_publisher_->msg_.output.accelerations.resize(dof_); + } + if (has_effort_command_interface_) + { + state_publisher_->msg_.output.effort.resize(dof_); + } + state_publisher_->unlock(); // action server configuration @@ -749,6 +841,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point_command(command_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); @@ -929,21 +1022,25 @@ void JointTrajectoryController::publish_state( if (state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = 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_.reference.positions = desired_state.positions; + state_publisher_->msg_.reference.velocities = desired_state.velocities; + state_publisher_->msg_.reference.accelerations = desired_state.accelerations; + state_publisher_->msg_.feedback.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_.feedback.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_.feedback.accelerations = current_state.accelerations; state_publisher_->msg_.error.accelerations = state_error.accelerations; } + if (read_commands_from_command_interfaces(command_current_)) + { + state_publisher_->msg_.output = command_current_; + } state_publisher_->unlockAndPublish(); } @@ -1315,6 +1412,27 @@ void JointTrajectoryController::resize_joint_trajectory_point( } } +void JointTrajectoryController::resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + if (has_position_command_interface_) + { + point.positions.resize(size, 0.0); + } + if (has_velocity_command_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_command_interface_) + { + point.accelerations.resize(size, 0.0); + } + if (has_effort_command_interface_) + { + point.effort.resize(size, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 1c0dbb5482..e8073086dc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -29,7 +29,6 @@ #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" @@ -49,10 +48,11 @@ #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" +#include "test_trajectory_controller_utils.hpp" + using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; @@ -469,38 +469,83 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) 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); + // No trajectory by default, no reference state or error + EXPECT_TRUE( + state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); EXPECT_TRUE( - state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); + state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); - EXPECT_EQ(n_joints, state->actual.positions.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); + + // expect feedback including all state_interfaces + EXPECT_EQ(n_joints, state->feedback.positions.size()); if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == state_interface_types_.end()) { - EXPECT_TRUE(state->actual.velocities.empty()); + EXPECT_TRUE(state->feedback.velocities.empty()); } else { - EXPECT_EQ(n_joints, state->actual.velocities.size()); + EXPECT_EQ(n_joints, state->feedback.velocities.size()); } if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == state_interface_types_.end()) { - EXPECT_TRUE(state->actual.accelerations.empty()); + EXPECT_TRUE(state->feedback.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state->actual.accelerations.size()); + EXPECT_EQ(n_joints, state->feedback.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); + // expect output including all command_interfaces + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.positions.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.positions.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.velocities.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.accelerations.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.effort.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.effort.size()); + } } // Floating-point value comparison threshold @@ -540,25 +585,25 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_msg->feedback.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->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.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); + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.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); + state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( - state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR( - state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { @@ -566,28 +611,34 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) 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); + EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->output.positions[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]); + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, state_msg->output.velocities[0]); + EXPECT_LT(0.0, state_msg->output.velocities[1]); + EXPECT_LT(0.0, state_msg->output.velocities[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 * (state_msg->reference.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 * (state_msg->reference.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 * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], k_p * allowed_delta); } } @@ -595,9 +646,12 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) 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]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + EXPECT_LT(0.0, state_msg->output.effort[0]); + EXPECT_LT(0.0, state_msg->output.effort[1]); + EXPECT_LT(0.0, state_msg->output.effort[2]); } executor.cancel(); @@ -638,26 +692,26 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_msg->feedback.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->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.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); + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.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); + state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( - state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + state_msg->error.positions[1], state_msg->reference.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); + state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); if (traj_controller_->has_position_command_interface()) { @@ -670,38 +724,38 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) 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_LT(0.0, joint_vel_[0]); + EXPECT_LT(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]); + EXPECT_GT(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 * (state_msg->reference.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 * (state_msg->reference.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 * (state_msg->reference.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]); + EXPECT_LT(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]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); } executor.cancel(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 25612f56e9..c85eb59f58 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -281,7 +281,7 @@ class TrajectoryControllerTest : public ::testing::Test // 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, + controller_name_ + "/controller_state", qos, [&](std::shared_ptr msg) { std::lock_guard guard(state_mutex_); @@ -385,20 +385,21 @@ class TrajectoryControllerTest : public ::testing::Test 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 + // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.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 + // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + EXPECT_NEAR( + expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); } } }