From 4d09c750c54bd4add33cdc8c35a489253da56755 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 10 May 2023 08:48:27 +0200 Subject: [PATCH 01/10] minimal changes jtc inherits from chainable interface, compiling not working --- .../joint_trajectory_controller.hpp | 16 +- .../joint_trajectory_plugin.xml | 2 +- .../src/joint_trajectory_controller.cpp | 979 +++++++++--------- 3 files changed, 506 insertions(+), 491 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index dfbdf7436e..05e5c2824f 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 @@ -26,7 +26,7 @@ #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" #include "control_toolbox/pid.hpp" -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_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" @@ -64,7 +64,7 @@ namespace joint_trajectory_controller { class Trajectory; -class JointTrajectoryController : public controller_interface::ControllerInterface +class JointTrajectoryController : public controller_interface::ChainableControllerInterface { public: JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -84,10 +84,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa 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; @@ -115,7 +111,15 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; // 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_ = { diff --git a/joint_trajectory_controller/joint_trajectory_plugin.xml b/joint_trajectory_controller/joint_trajectory_plugin.xml index 27930380ea..a9f0647a60 100644 --- a/joint_trajectory_controller/joint_trajectory_plugin.xml +++ b/joint_trajectory_controller/joint_trajectory_plugin.xml @@ -1,5 +1,5 @@ - + The joint trajectory controller executes joint-space trajectories on a set of joints diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 07080bb1fc..52a5fa1cb4 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -46,7 +46,7 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0) +: controller_interface::ChainableControllerInterface(), dof_(0) { } @@ -108,542 +108,284 @@ JointTrajectoryController::state_interface_configuration() const return conf; } -controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) +void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) { - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - return controller_interface::return_type::OK; + 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(); } +} - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) +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) { - // error defined as the difference between current and desired - if (normalize_joint_error_[index]) + for (size_t index = 0; index < dof_; ++index) { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piget_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) + } + else { - 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); - // set the active trajectory pointer to the new goal - traj_point_active_ptr_ = &traj_external_point_ptr_; + state.accelerations.clear(); } - // 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) + 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) { - joint_interface[index].get().set_value(trajectory_point_interface[index]); + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; - // current state update - state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); + 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(); + }; - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + // Assign values from the command interfaces as command. + if (has_position_command_interface_) { - bool first_sample = false; - // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) + if (interface_has_values(joint_command_interface_[0])) { - 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_); - } + 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; } + } - // 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); + return has_values; +} - if (valid_point) +void JointTrajectoryController::query_state_service( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto logger = get_node()->get_logger(); + // Preconditions + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); + response->success = false; + return; + } + const auto active_goal = *rt_active_goal_.readFromRT(); + response->name = params_.joints; + trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; + if ((traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())) + { + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + response->success = (*traj_point_active_ptr_) + ->sample( + static_cast(request->time), interpolation_method_, + state_requested, start_segment_itr, end_segment_itr); + // If the requested sample time precedes the trajectory finish time respond as failure + if (response->success) { - 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) + if (end_segment_itr == (*traj_point_active_ptr_)->end()) { - 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_error_.positions[i], state_error_.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()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; - - // 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()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; - - 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"); + RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); + response->success = false; } } - } - - publish_state(time, 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(); + RCLCPP_ERROR( + logger, "Requested sample time is earlier than the current trajectory start time."); } } else { - // Make empty so the property is ignored during interpolation - state.velocities.clear(); - state.accelerations.clear(); + RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance."); + response->success = false; } + response->position = state_requested.positions; + response->velocity = state_requested.velocities; + response->acceleration = state_requested.accelerations; } -bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) +controller_interface::CallbackReturn JointTrajectoryController::on_configure( + const rclcpp_lifecycle::State &) { - bool has_values = true; + const auto logger = get_node()->get_logger(); - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) + if (!param_listener_) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } - 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(); - }; + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); - // 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])) + // 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_)) { - assign_point_from_interface(state.positions, joint_command_interface_[0]); + 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; } - else + + dof_ = params_.joints.size(); + + // TODO(destogl): why is this here? Add comment or move + if (!reset()) { - state.positions.clear(); - has_values = false; + return CallbackReturn::FAILURE; } - // velocity and acceleration states are optional - if (has_velocity_state_interface_) + + if (params_.joints.empty()) { - 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; - } + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? + RCLCPP_WARN(logger, "'joints' parameter is empty."); } - else + + command_joint_names_ = params_.command_joints; + + if (command_joint_names_.empty()) { - 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; -} - -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) -{ - const auto logger = get_node()->get_logger(); - // Preconditions - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); - response->success = false; - return; - } - const auto active_goal = *rt_active_goal_.readFromRT(); - response->name = params_.joints; - trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; - if ((traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())) - { - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - response->success = (*traj_point_active_ptr_) - ->sample( - static_cast(request->time), interpolation_method_, - state_requested, start_segment_itr, end_segment_itr); - // If the requested sample time precedes the trajectory finish time respond as failure - if (response->success) - { - if (end_segment_itr == (*traj_point_active_ptr_)->end()) - { - RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); - response->success = false; - } - } - else - { - RCLCPP_ERROR( - logger, "Requested sample time is earlier than the current trajectory start time."); - } - } - else - { - RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance."); - response->success = false; - } - response->position = state_requested.positions; - response->velocity = state_requested.velocities; - response->acceleration = state_requested.accelerations; -} - -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."); + 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()) { @@ -1021,6 +763,274 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( return CallbackReturn::SUCCESS; } +controller_interface::return_type JointTrajectoryController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ +} + +std::vector +JointTrajectoryController::on_export_reference_interfaces() +{ +} + +controller_interface::return_type JointTrajectoryController::update_reference_from_subscribers( + 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 + if (normalize_joint_error_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -piget_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); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; + } + + // 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_error_.positions[i], state_error_.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()); + // remove the active trajectory pointer so that we stop commanding the hardware + traj_point_active_ptr_ = nullptr; + + // 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()); + // remove the active trajectory pointer so that we stop commanding the hardware + traj_point_active_ptr_ = nullptr; + + 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"); + } + } + } + + publish_state(time, state_desired_, state_current_, state_error_); + return controller_interface::return_type::OK; +} + void JointTrajectoryController::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) @@ -1444,4 +1454,5 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) + joint_trajectory_controller::JointTrajectoryController, + controller_interface::ChainableControllerInterface) From 2577680cac1eeb949c439f3da02826ace3951153 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Sat, 13 May 2023 09:40:18 +0200 Subject: [PATCH 02/10] first poc for chainable jtc --- .../joint_trajectory_controller.hpp | 28 ++- .../src/joint_trajectory_controller.cpp | 159 ++++++++++++++++-- 2 files changed, 167 insertions(+), 20 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 05e5c2824f..7ae3a8fbc8 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 @@ -111,11 +111,13 @@ class JointTrajectoryController : public controller_interface::ChainableControll controller_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - protected: + // internal reference values + std::vector> position_reference_; + std::vector> velocity_reference_; + std::vector> acceleration_reference_; + std::vector> effort_reference_; + std::vector on_export_reference_interfaces() override; controller_interface::return_type update_reference_from_subscribers( @@ -129,6 +131,24 @@ class JointTrajectoryController : public controller_interface::ChainableControll hardware_interface::HW_IF_EFFORT, }; + template + void copy_reference_interfaces_values( + std::vector & msg_container, std::vector> & reference_input); + + /** + * If controller is in chained mode we create new JointTrajectory msgs + * from the reference interface input. + * If controller is NOT in chained mode we get the JointTrajectory msg + * from topic. + * + * For detailed explanation on this have a look at the update_reference_from_subscribers + * method which normally would handle such task + */ + void update_joint_trajectory_point_from_input(); + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; trajectory_msgs::msg::JointTrajectoryPoint command_current_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 52a5fa1cb4..2f7de83930 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,6 +43,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "std_msgs/msg/header.hpp" +#include "rclcpp/rclcpp.hpp" namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() @@ -763,17 +764,154 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( return CallbackReturn::SUCCESS; } -controller_interface::return_type JointTrajectoryController::update_and_write_commands( +std::vector +JointTrajectoryController::on_export_reference_interfaces() +{ + // TODO(Manuel) : only for fast poc export every available command_interface + // as chainable + std::vector chainable_command_interfaces; + const auto command_interfaces = params_.command_interfaces; + const auto joints = params_.joints; + const auto num_chainable_interfaces = joints.size() * command_interfaces.size(); + + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("A"), "command_interfaces size:" << num_chainable_interfaces); + + // allocate dynamic memory + chainable_command_interfaces.reserve(num_chainable_interfaces); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + + // assign reference interfaces + auto index = 0ul; + for (const auto & interface : command_interfaces) + { + for (const auto & joint : joints) + { + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_[index]); + else if (hardware_interface::HW_IF_VELOCITY == interface) + { + velocity_reference_.emplace_back(reference_interfaces_[index]); + } + else if (hardware_interface::HW_IF_ACCELERATION == interface) + { + acceleration_reference_.emplace_back(reference_interfaces_[index]); + } + else if (hardware_interface::HW_IF_EFFORT == interface) + { + effort_reference_.emplace_back(reference_interfaces_[index]); + } + const auto full_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + + ++index; + } + } + + return chainable_command_interfaces; +} + +controller_interface::return_type JointTrajectoryController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) { + // Updating the reference interface from the subscriber doesn't make much sense for jtc in my opinion. + // This is due to the fact that a JointTrajecorty msg can have multiple JointTrajectoryPoint points + // This way we would need to have multi dimensional reference interface for each interface type: + // ( position, velocity ,...). + // E.g. msg1{[JointTrajectoryPoint 1, JointTrajectoryPoint 2, ... ,JointTrajectoryPoint n]}. + // - JointTrajectoryPoint 1{position 1, velocity 1} + // .... + // - JointTrajectoryPoint n{position n, velocity n} + // => reference interface for position would need to get passed position 1 - position n. + // It is easier to check in the update_and_write_commands() if we are currently in chained mode or not. + // if we are in chained mode then create a JointTrajecorty msg and do calculation like in external mode. + + // Code example to make the problem clear: + // note not working since this example would only pass contents of JointTrajectoryPoint n. + + // auto external_joint_trajectory = traj_msg_external_point_ptr_.readFromRT(); + // if (!external_joint_trajectory->get()) + // { + // // Is this correct? + // return controller_interface::return_type::ERROR; + // } + + // for (const auto point : external_joint_trajectory->get()->points) + // { + // for (size_t i = 0; i < point.positions.size(); ++i) + // { + // { + // position_reference_[i].get() = point.positions[i]; + // } + // for (size_t i = 0; i < point.velocities.size(); ++i) + // { + // velocity_reference_[i].get() = point.velocities[i]; + // } + // for (size_t i = 0; i < point.accelerations.size(); ++i) + // { + // acceleration_reference_[i].get() = point.velocities[i]; + // } + // for (size_t i = 0; i < point.effort.size(); ++i) + // { + // effort_reference_[i].get() = point.velocities[i]; + // } + // } + // } + + return controller_interface::return_type::OK; } -std::vector -JointTrajectoryController::on_export_reference_interfaces() +template +void JointTrajectoryController::copy_reference_interfaces_values( + std::vector & msg_container, std::vector> & reference_input) { + msg_container.resize(reference_input.size()); + std::transform( + reference_input.begin(), reference_input.end(), msg_container.begin(), + [](const std::reference_wrapper & ref) { return ref.get(); }); } -controller_interface::return_type JointTrajectoryController::update_reference_from_subscribers( +void JointTrajectoryController::update_joint_trajectory_point_from_input() +{ + auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); + std::shared_ptr new_trajecotry_msg; + if (is_in_chained_mode()) + { + // if we are in chained mode we create a trajectory msg from the reference + // interface input. For background on why look at the comment in the + // update_reference_from_subscribers(...) function + auto trajectory_point = trajectory_msgs::msg::JointTrajectoryPoint(); + // file with input from reference interfaces + copy_reference_interfaces_values(trajectory_point.positions, position_reference_); + copy_reference_interfaces_values(trajectory_point.velocities, velocity_reference_); + copy_reference_interfaces_values(trajectory_point.accelerations, acceleration_reference_); + copy_reference_interfaces_values(trajectory_point.effort, effort_reference_); + + auto trajectory = trajectory_msgs::msg::JointTrajectory::SharedPtr(); + trajectory->points.push_back(trajectory_point); + new_trajecotry_msg = trajectory; + } + else + { + // if not in chained mode + // get new JointTrajectory msg from external subscription + new_trajecotry_msg = *(traj_msg_external_point_ptr_.readFromRT()); + } + + // Check if a new external message has been received from nonRT threads + if (current_external_msg != new_trajecotry_msg) + { + fill_partial_goal(new_trajecotry_msg); + sort_to_local_joint_order(new_trajecotry_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(new_trajecotry_msg); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; + } +} + +controller_interface::return_type JointTrajectoryController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -808,18 +946,7 @@ controller_interface::return_type JointTrajectoryController::update_reference_fr } }; - // 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); - // set the active trajectory pointer to the new goal - traj_point_active_ptr_ = &traj_external_point_ptr_; - } + update_joint_trajectory_point_from_input(); // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? From ee73ab3886e64b5ec000dd9f2dbe540697018419 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 16 May 2023 14:38:18 +0200 Subject: [PATCH 03/10] roughly working chainedable jtc. Things to consider/improve: * Duration of trajecotry execution is hardcoded * Check for new trajecotry is done via iterating over reference_interfaces_. Double comparison... * is check for fill_parital_goals and sort_to_local_joint_order in chained mode necassary? --- .../joint_trajectory_controller.hpp | 8 +- .../src/joint_trajectory_controller.cpp | 129 ++++++++++++------ 2 files changed, 92 insertions(+), 45 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 7ae3a8fbc8..77b0e5ccf7 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 @@ -117,6 +117,10 @@ class JointTrajectoryController : public controller_interface::ChainableControll std::vector> velocity_reference_; std::vector> acceleration_reference_; std::vector> effort_reference_; + std::vector joint_names_; + std::vector last_references_ = {}; + bool first_time_ = true; + rclcpp::Time last_time_; std::vector on_export_reference_interfaces() override; @@ -131,6 +135,8 @@ class JointTrajectoryController : public controller_interface::ChainableControll hardware_interface::HW_IF_EFFORT, }; + bool reference_changed(); + template void copy_reference_interfaces_values( std::vector & msg_container, std::vector> & reference_input); @@ -144,7 +150,7 @@ class JointTrajectoryController : public controller_interface::ChainableControll * For detailed explanation on this have a look at the update_reference_from_subscribers * method which normally would handle such task */ - void update_joint_trajectory_point_from_input(); + void update_joint_trajectory_point_from_input(const rclcpp::Time & time); controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 2f7de83930..0d7661cd59 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -774,19 +774,23 @@ JointTrajectoryController::on_export_reference_interfaces() const auto joints = params_.joints; const auto num_chainable_interfaces = joints.size() * command_interfaces.size(); - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("A"), "command_interfaces size:" << num_chainable_interfaces); - // allocate dynamic memory chainable_command_interfaces.reserve(num_chainable_interfaces); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + position_reference_ = {}; + velocity_reference_ = {}; + acceleration_reference_ = {}; + effort_reference_ = {}; + joint_names_ = {}; + // assign reference interfaces auto index = 0ul; for (const auto & interface : command_interfaces) { for (const auto & joint : joints) { + joint_names_.push_back(joint); if (hardware_interface::HW_IF_POSITION == interface) position_reference_.emplace_back(reference_interfaces_[index]); else if (hardware_interface::HW_IF_VELOCITY == interface) @@ -816,7 +820,7 @@ controller_interface::return_type JointTrajectoryController::update_reference_fr const rclcpp::Time & time, const rclcpp::Duration & period) { // Updating the reference interface from the subscriber doesn't make much sense for jtc in my opinion. - // This is due to the fact that a JointTrajecorty msg can have multiple JointTrajectoryPoint points + // This is due to the fact that a JointTrajectory msg can have multiple JointTrajectoryPoint points // This way we would need to have multi dimensional reference interface for each interface type: // ( position, velocity ,...). // E.g. msg1{[JointTrajectoryPoint 1, JointTrajectoryPoint 2, ... ,JointTrajectoryPoint n]}. @@ -825,10 +829,12 @@ controller_interface::return_type JointTrajectoryController::update_reference_fr // - JointTrajectoryPoint n{position n, velocity n} // => reference interface for position would need to get passed position 1 - position n. // It is easier to check in the update_and_write_commands() if we are currently in chained mode or not. - // if we are in chained mode then create a JointTrajecorty msg and do calculation like in external mode. + // if we are in chained mode then create a JointTrajectory msg and do calculation like in external mode. - // Code example to make the problem clear: - // note not working since this example would only pass contents of JointTrajectoryPoint n. + //****************************************************************************************************** + // Code example to make the problem clear: * + // note not working since this example would only pass contents of JointTrajectoryPoint n. * + //****************************************************************************************************** // auto external_joint_trajectory = traj_msg_external_point_ptr_.readFromRT(); // if (!external_joint_trajectory->get()) @@ -836,7 +842,6 @@ controller_interface::return_type JointTrajectoryController::update_reference_fr // // Is this correct? // return controller_interface::return_type::ERROR; // } - // for (const auto point : external_joint_trajectory->get()->points) // { // for (size_t i = 0; i < point.positions.size(); ++i) @@ -872,42 +877,76 @@ void JointTrajectoryController::copy_reference_interfaces_values( [](const std::reference_wrapper & ref) { return ref.get(); }); } -void JointTrajectoryController::update_joint_trajectory_point_from_input() +// TODO(Manuel): just for testing should not be merged, not a good solution +bool JointTrajectoryController::reference_changed() { - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - std::shared_ptr new_trajecotry_msg; - if (is_in_chained_mode()) + if (last_references_.size() != reference_interfaces_.size()) { - // if we are in chained mode we create a trajectory msg from the reference - // interface input. For background on why look at the comment in the - // update_reference_from_subscribers(...) function - auto trajectory_point = trajectory_msgs::msg::JointTrajectoryPoint(); - // file with input from reference interfaces - copy_reference_interfaces_values(trajectory_point.positions, position_reference_); - copy_reference_interfaces_values(trajectory_point.velocities, velocity_reference_); - copy_reference_interfaces_values(trajectory_point.accelerations, acceleration_reference_); - copy_reference_interfaces_values(trajectory_point.effort, effort_reference_); + last_references_ = reference_interfaces_; + return true; + } + for (int i = 0; i < last_references_.size(); ++i) + { + if (last_references_[i] != reference_interfaces_[i]) + { + last_references_ = reference_interfaces_; + return true; + } + } + return false; +} - auto trajectory = trajectory_msgs::msg::JointTrajectory::SharedPtr(); - trajectory->points.push_back(trajectory_point); - new_trajecotry_msg = trajectory; +void JointTrajectoryController::update_joint_trajectory_point_from_input(const rclcpp::Time & time) +{ + std::shared_ptr current_external_msg = + traj_external_point_ptr_->get_trajectory_msg(); + std::shared_ptr new_trajectory_msg; + if (is_in_chained_mode()) + { + if (reference_changed()) + { + // if we are in chained mode we create a trajectory msg from the reference + // interface input. For background on why look at the comment in the + // update_reference_from_subscribers(...) function + auto trajectory_point = trajectory_msgs::msg::JointTrajectoryPoint(); + auto duration = builtin_interfaces::msg::Duration(); + duration.sec = 2; + trajectory_point.time_from_start = duration; + + copy_reference_interfaces_values(trajectory_point.positions, position_reference_); + copy_reference_interfaces_values(trajectory_point.velocities, velocity_reference_); + copy_reference_interfaces_values(trajectory_point.accelerations, acceleration_reference_); + copy_reference_interfaces_values(trajectory_point.effort, effort_reference_); + + std::shared_ptr trajectory = + std::make_shared(); + + trajectory->joint_names = joint_names_; + trajectory->points.push_back(trajectory_point); + new_trajectory_msg = trajectory; + + fill_partial_goal(new_trajectory_msg); + sort_to_local_joint_order(new_trajectory_msg); + traj_external_point_ptr_->update(new_trajectory_msg); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; + } } else { // if not in chained mode // get new JointTrajectory msg from external subscription - new_trajecotry_msg = *(traj_msg_external_point_ptr_.readFromRT()); - } - - // Check if a new external message has been received from nonRT threads - if (current_external_msg != new_trajecotry_msg) - { - fill_partial_goal(new_trajecotry_msg); - sort_to_local_joint_order(new_trajecotry_msg); - // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(new_trajecotry_msg); - // set the active trajectory pointer to the new goal - traj_point_active_ptr_ = &traj_external_point_ptr_; + new_trajectory_msg = *(traj_msg_external_point_ptr_.readFromRT()); + // Check if a new external message has been received from nonRT threads + if (current_external_msg != new_trajectory_msg) + { + fill_partial_goal(new_trajectory_msg); + sort_to_local_joint_order(new_trajectory_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(new_trajectory_msg); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; + } } } @@ -946,7 +985,7 @@ controller_interface::return_type JointTrajectoryController::update_and_write_co } }; - update_joint_trajectory_point_from_input(); + update_joint_trajectory_point_from_input(time); // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? @@ -1343,8 +1382,8 @@ void JointTrajectoryController::sort_to_local_joint_order( // 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 + const std::vector & to_remap, const std::vector & mapping, + const std::string & type) -> std::vector { if (to_remap.empty()) { @@ -1353,7 +1392,9 @@ void JointTrajectoryController::sort_to_local_joint_order( if (to_remap.size() != mapping.size()) { RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + get_node()->get_logger(), + "Invalid input size (%zu) for sorting with mapping size (%zu) for %s", to_remap.size(), + mapping.size(), type.c_str()); return to_remap; } std::vector output; @@ -1369,16 +1410,16 @@ void JointTrajectoryController::sort_to_local_joint_order( for (size_t index = 0; index < trajectory_msg->points.size(); ++index) { trajectory_msg->points[index].positions = - remap(trajectory_msg->points[index].positions, mapping_vector); + remap(trajectory_msg->points[index].positions, mapping_vector, "positions"); trajectory_msg->points[index].velocities = - remap(trajectory_msg->points[index].velocities, mapping_vector); + remap(trajectory_msg->points[index].velocities, mapping_vector, "velocities"); trajectory_msg->points[index].accelerations = - remap(trajectory_msg->points[index].accelerations, mapping_vector); + remap(trajectory_msg->points[index].accelerations, mapping_vector, "accelerations"); trajectory_msg->points[index].effort = - remap(trajectory_msg->points[index].effort, mapping_vector); + remap(trajectory_msg->points[index].effort, mapping_vector, "effort"); } } From f20397bad15593464166320ba77241102a151edc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 21 Nov 2022 15:00:41 +0100 Subject: [PATCH 04/10] adapt to changes made to loandedInterface --- .../test/test_diff_drive_controller.cpp | 20 ++++--- .../test_force_torque_sensor_broadcaster.hpp | 24 ++++++--- .../test/test_imu_sensor_broadcaster.hpp | 50 +++++++++++------- .../test/test_joint_state_broadcaster.hpp | 52 +++++++++++-------- .../test/test_trajectory_controller_utils.hpp | 12 ++--- .../test/test_tricycle_controller.cpp | 10 ++-- 6 files changed, 103 insertions(+), 65 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..d046418c90 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -163,14 +163,18 @@ class TestDiffDriveController : public ::testing::Test std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; - hardware_interface::StateInterface left_wheel_pos_state_{ - left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; - hardware_interface::StateInterface right_wheel_pos_state_{ - right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; - hardware_interface::StateInterface left_wheel_vel_state_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::StateInterface right_wheel_vel_state_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + std::shared_ptr left_wheel_pos_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]); + std::shared_ptr right_wheel_pos_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]); + std::shared_ptr left_wheel_vel_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + std::shared_ptr right_wheel_vel_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); hardware_interface::CommandInterface left_wheel_vel_cmd_{ left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; hardware_interface::CommandInterface right_wheel_vel_cmd_{ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 5477b3fa6f..46ede8f606 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -57,12 +57,24 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test const std::string frame_id_ = "fts_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; - hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; - hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]}; - hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]}; - hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]}; - hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]}; + std::shared_ptr fts_force_x_ = + std::make_shared( + sensor_name_, "force.x", &sensor_values_[0]); + std::shared_ptr fts_force_y_ = + std::make_shared( + sensor_name_, "force.y", &sensor_values_[1]); + std::shared_ptr fts_force_z_ = + std::make_shared( + sensor_name_, "force.z", &sensor_values_[2]); + std::shared_ptr fts_torque_x_ = + std::make_shared( + sensor_name_, "torque.x", &sensor_values_[3]); + std::shared_ptr fts_torque_y_ = + std::make_shared( + sensor_name_, "torque.y", &sensor_values_[4]); + std::shared_ptr fts_torque_z_ = + std::make_shared( + sensor_name_, "torque.z", &sensor_values_[5]); std::unique_ptr fts_broadcaster_; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..8a3feb98ca 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -55,26 +55,36 @@ class IMUSensorBroadcasterTest : public ::testing::Test const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - hardware_interface::StateInterface imu_orientation_x_{ - sensor_name_, "orientation.x", &sensor_values_[0]}; - hardware_interface::StateInterface imu_orientation_y_{ - sensor_name_, "orientation.y", &sensor_values_[1]}; - hardware_interface::StateInterface imu_orientation_z_{ - sensor_name_, "orientation.z", &sensor_values_[2]}; - hardware_interface::StateInterface imu_orientation_w_{ - sensor_name_, "orientation.w", &sensor_values_[3]}; - hardware_interface::StateInterface imu_angular_velocity_x_{ - sensor_name_, "angular_velocity.x", &sensor_values_[4]}; - hardware_interface::StateInterface imu_angular_velocity_y_{ - sensor_name_, "angular_velocity.y", &sensor_values_[5]}; - hardware_interface::StateInterface imu_angular_velocity_z_{ - sensor_name_, "angular_velocity.z", &sensor_values_[6]}; - hardware_interface::StateInterface imu_linear_acceleration_x_{ - sensor_name_, "linear_acceleration.x", &sensor_values_[7]}; - hardware_interface::StateInterface imu_linear_acceleration_y_{ - sensor_name_, "linear_acceleration.y", &sensor_values_[8]}; - hardware_interface::StateInterface imu_linear_acceleration_z_{ - sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; + std::shared_ptr imu_orientation_x_ = + std::make_shared( + sensor_name_, "orientation.x", &sensor_values_[0]); + std::shared_ptr imu_orientation_y_ = + std::make_shared( + sensor_name_, "orientation.y", &sensor_values_[1]); + std::shared_ptr imu_orientation_z_ = + std::make_shared( + sensor_name_, "orientation.z", &sensor_values_[2]); + std::shared_ptr imu_orientation_w_ = + std::make_shared( + sensor_name_, "orientation.w", &sensor_values_[3]); + std::shared_ptr imu_angular_velocity_x_ = + std::make_shared( + sensor_name_, "angular_velocity.x", &sensor_values_[4]); + std::shared_ptr imu_angular_velocity_y_ = + std::make_shared( + sensor_name_, "angular_velocity.y", &sensor_values_[5]); + std::shared_ptr imu_angular_velocity_z_ = + std::make_shared( + sensor_name_, "angular_velocity.z", &sensor_values_[6]); + std::shared_ptr imu_linear_acceleration_x_ = + std::make_shared( + sensor_name_, "linear_acceleration.x", &sensor_values_[7]); + std::shared_ptr imu_linear_acceleration_y_ = + std::make_shared( + sensor_name_, "linear_acceleration.y", &sensor_values_[8]); + std::shared_ptr imu_linear_acceleration_z_ = + std::make_shared( + sensor_name_, "linear_acceleration.z", &sensor_values_[9]); std::unique_ptr imu_broadcaster_; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..e5372233de 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -79,27 +79,37 @@ class JointStateBroadcasterTest : public ::testing::Test std::vector joint_values_ = {1.1, 2.1, 3.1}; double custom_joint_value_ = 3.5; - hardware_interface::StateInterface joint_1_pos_state_{ - joint_names_[0], interface_names_[0], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_pos_state_{ - joint_names_[1], interface_names_[0], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_pos_state_{ - joint_names_[2], interface_names_[0], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_vel_state_{ - joint_names_[0], interface_names_[1], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_vel_state_{ - joint_names_[1], interface_names_[1], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_vel_state_{ - joint_names_[2], interface_names_[1], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_eff_state_{ - joint_names_[0], interface_names_[2], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_eff_state_{ - joint_names_[1], interface_names_[2], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_eff_state_{ - joint_names_[2], interface_names_[2], &joint_values_[2]}; - - hardware_interface::StateInterface joint_X_custom_state{ - joint_names_[0], custom_interface_name_, &custom_joint_value_}; + std::shared_ptr joint_1_pos_state_ = + std::make_shared( + joint_names_[0], interface_names_[0], &joint_values_[0]); + std::shared_ptr joint_2_pos_state_ = + std::make_shared( + joint_names_[1], interface_names_[0], &joint_values_[1]); + std::shared_ptr joint_3_pos_state_ = + std::make_shared( + joint_names_[2], interface_names_[0], &joint_values_[2]); + std::shared_ptr joint_1_vel_state_ = + std::make_shared( + joint_names_[0], interface_names_[1], &joint_values_[0]); + std::shared_ptr joint_2_vel_state_ = + std::make_shared( + joint_names_[1], interface_names_[1], &joint_values_[1]); + std::shared_ptr joint_3_vel_state_ = + std::make_shared( + joint_names_[2], interface_names_[1], &joint_values_[2]); + std::shared_ptr joint_1_eff_state_ = + std::make_shared( + joint_names_[0], interface_names_[2], &joint_values_[0]); + std::shared_ptr joint_2_eff_state_ = + std::make_shared( + joint_names_[1], interface_names_[2], &joint_values_[1]); + std::shared_ptr joint_3_eff_state_ = + std::make_shared( + joint_names_[2], interface_names_[2], &joint_values_[2]); + + std::shared_ptr joint_X_custom_state = + std::make_shared( + joint_names_[0], custom_interface_name_, &custom_joint_value_); std::unique_ptr state_broadcaster_; }; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c85eb59f58..7b22e0638b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -237,13 +237,13 @@ class TrajectoryControllerTest : public ::testing::Test 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( + pos_state_interfaces_.emplace_back(std::make_shared( 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( + vel_state_interfaces_.emplace_back(std::make_shared( 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( + acc_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); @@ -437,9 +437,9 @@ class TrajectoryControllerTest : public ::testing::Test 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_; + 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 diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..f75e868dc5 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -153,11 +153,13 @@ class TestTricycleController : public ::testing::Test double position_ = 0.1; double velocity_ = 0.2; - hardware_interface::StateInterface steering_joint_pos_state_{ - steering_joint_name, HW_IF_POSITION, &position_}; + std::shared_ptr steering_joint_pos_state_ = + std::make_shared( + steering_joint_name, HW_IF_POSITION, &position_); - hardware_interface::StateInterface traction_joint_vel_state_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + std::shared_ptr traction_joint_vel_state_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); hardware_interface::CommandInterface steering_joint_pos_cmd_{ steering_joint_name, HW_IF_POSITION, &position_}; From 0868154034bc85bd8d9df114cfcd2008928ce3b1 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 5 Dec 2022 10:56:41 +0100 Subject: [PATCH 05/10] adjust tests to reflect changes in handles --- .../test/test_diff_drive_controller.cpp | 30 +++++----- .../test_joint_group_effort_controller.cpp | 48 +++++++-------- .../test_joint_group_effort_controller.hpp | 9 ++- .../test/test_forward_command_controller.cpp | 60 +++++++++---------- .../test/test_forward_command_controller.hpp | 9 ++- ...i_interface_forward_command_controller.cpp | 48 +++++++-------- ...i_interface_forward_command_controller.hpp | 9 ++- .../test/test_trajectory_controller_utils.hpp | 16 ++--- .../test_joint_group_position_controller.cpp | 36 +++++------ .../test_joint_group_position_controller.hpp | 9 ++- .../test/test_tricycle_controller.cpp | 30 +++++----- .../test_joint_group_velocity_controller.cpp | 48 +++++++-------- .../test_joint_group_velocity_controller.hpp | 9 ++- 13 files changed, 190 insertions(+), 171 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index d046418c90..2c710b05a8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -175,10 +175,12 @@ class TestDiffDriveController : public ::testing::Test std::shared_ptr right_wheel_vel_state_ = std::make_shared( right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); - hardware_interface::CommandInterface left_wheel_vel_cmd_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::CommandInterface right_wheel_vel_cmd_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + std::shared_ptr left_wheel_vel_cmd_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + std::shared_ptr right_wheel_vel_cmd_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -372,8 +374,8 @@ TEST_F(TestDiffDriveController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()); executor.cancel(); } @@ -397,8 +399,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -413,8 +415,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(1.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(1.0, right_wheel_vel_cmd_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -425,14 +427,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 256a4ce465..d4af71f3b0 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,9 +214,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -228,15 +228,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 0.0); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..160ebb1309 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -54,9 +54,12 @@ class JointGroupEffortControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + std::shared_ptr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]); + std::shared_ptr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]); + std::shared_ptr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..6ec16f4717 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -195,9 +195,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -210,9 +210,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -238,9 +238,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -260,9 +260,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -273,9 +273,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -303,9 +303,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -316,9 +316,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -337,9 +337,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -381,9 +381,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -394,7 +394,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 7.7); } diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..61e454ecd8 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -66,9 +66,12 @@ class ForwardCommandControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + std::shared_ptr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + std::shared_ptr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 0cada04859..ae7efd6141 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -189,9 +189,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -209,9 +209,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -224,9 +224,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -244,9 +244,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -273,9 +273,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -293,9 +293,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -336,9 +336,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -349,7 +349,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 7.7); } diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..e427776bab 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -74,9 +74,12 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test double vel_cmd_ = 2.1; double eff_cmd_ = 3.1; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; - CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; - CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &pos_cmd_); + std::shared_ptr joint_1_vel_cmd_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &vel_cmd_); + std::shared_ptr joint_1_eff_cmd_ = + std::make_shared(joint_name_, HW_IF_EFFORT, &eff_cmd_); }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7b22e0638b..df1cc20fac 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -228,13 +228,13 @@ class TrajectoryControllerTest : public ::testing::Test 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( + pos_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); - vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + vel_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); - acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + acc_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); - eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + eff_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); pos_state_interfaces_.emplace_back(std::make_shared( @@ -433,10 +433,10 @@ class TrajectoryControllerTest : public ::testing::Test 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_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_; diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a712cc81c2..163f2b710b 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,7 +214,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..152c73bf23 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -55,9 +55,12 @@ class JointGroupPositionControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + std::shared_ptr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + std::shared_ptr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index f75e868dc5..53fb1c41d6 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -161,11 +161,13 @@ class TestTricycleController : public ::testing::Test std::make_shared( traction_joint_name, HW_IF_VELOCITY, &velocity_); - hardware_interface::CommandInterface steering_joint_pos_cmd_{ - steering_joint_name, HW_IF_POSITION, &position_}; + std::shared_ptr steering_joint_pos_cmd_ = + std::make_shared( + steering_joint_name, HW_IF_POSITION, &position_); - hardware_interface::CommandInterface traction_joint_vel_cmd_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + std::shared_ptr traction_joint_vel_cmd_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -284,8 +286,8 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()); executor.cancel(); } @@ -309,8 +311,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_->get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -325,8 +327,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -337,14 +339,14 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 43c1545461..8d350ec201 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,9 +214,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -228,15 +228,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 0.0); } diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..32854085be 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -55,9 +55,12 @@ class JointGroupVelocityControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + std::shared_ptr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]); + std::shared_ptr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]); + std::shared_ptr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ From dc1d3b578f8f73ef4d57182169d1267ccc90ff1d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 11 Jan 2023 08:58:49 +0100 Subject: [PATCH 06/10] joint_state_broadcaster if no local topic us "/" as prefix --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 74a19c9ed0..c449e9ae56 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -146,7 +146,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( try { - const std::string topic_name_prefix = params_.use_local_topics ? "~/" : ""; + const std::string topic_name_prefix = params_.use_local_topics ? "~/" : "/"; joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); From 372789f58ff97a5cf02d0165b287eae23c42abc9 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 11 Jan 2023 09:55:48 +0100 Subject: [PATCH 07/10] adapt admittance_controller tests --- .../test/test_admittance_controller.hpp | 10 +++++----- gripper_controllers/test/test_gripper_controllers.hpp | 9 ++++++--- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7dca2e2010..5a17f05102 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -201,7 +201,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_command_values_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( + command_itfs_.emplace_back(std::make_shared( joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } @@ -216,7 +216,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( + state_itfs_.emplace_back(std::make_shared( joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -226,7 +226,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( + state_itfs_.emplace_back(std::make_shared( ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -397,8 +397,8 @@ class AdmittanceControllerTest : public ::testing::Test std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_; - std::vector state_itfs_; - std::vector command_itfs_; + std::vector> state_itfs_; + std::vector> command_itfs_; // Test related parameters std::unique_ptr controller_; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 3e9750a603..b002aaf9c4 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -57,9 +57,12 @@ class GripperControllerTest : public ::testing::Test std::vector joint_states_ = {1.1, 2.1}; std::vector joint_commands_ = {3.1}; - StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; - StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; + std::shared_ptr joint_1_pos_state_ = + std::make_shared(joint_name_, HW_IF_POSITION, &joint_states_[0]); + std::shared_ptr joint_1_vel_state_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &joint_states_[1]); + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &joint_commands_[0]); }; #endif // TEST_GRIPPER_CONTROLLERS_HPP_ From 470ea5052c82a8b90dbecbd2f9915fee1fb0a9ac Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 28 Jun 2023 21:28:36 +0200 Subject: [PATCH 08/10] first working publishing of trajectory generated by moveit --- .../ros2_controllers_test_nodes/move_lin.py | 156 ++++++++++++++++++ ros2_controllers_test_nodes/setup.py | 2 + 2 files changed, 158 insertions(+) create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py new file mode 100644 index 0000000000..6f53393538 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py @@ -0,0 +1,156 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Manuel Muth +# + + +import rclpy +from rclpy.node import Node + +import yaml +from std_msgs.msg import Header +from builtin_interfaces.msg import Duration +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +class MoveLin(Node): + def __init__(self): + super().__init__("move_lin") + # Declare all parameters + self.declare_parameter("controller_name", "position_trajectory_controller") + self.declare_parameter("wait_sec_between_publish", 5) + self.declare_parameter("joints", [""]) + self.declare_parameter("goal_names", ["traj1", "traj2"]) + self.declare_parameter("check_starting_point", False) + + # Read parameters + wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + goal_names = self.get_parameter("goal_names").value + controller_name = self.get_parameter("controller_name").value + self.joints = self.get_parameter("joints").value + self.check_starting_point = self.get_parameter("check_starting_point").value + self.starting_point = {} + self.trajectories = [] + + # starting point stuff + if self.check_starting_point: + # declare nested params + for name in self.joints: + param_name_tmp = "starting_point_limits" + "." + name + self.declare_parameter(param_name_tmp, [-2 * 3.14159, 2 * 3.14159]) + self.starting_point[name] = self.get_parameter(param_name_tmp).value + + for name in self.joints: + if len(self.starting_point[name]) != 2: + raise Exception('"starting_point" parameter is not set correctly!') + self.joint_state_sub = self.create_subscription( + JointState, "joint_states", self.joint_state_callback, 10 + ) + # initialize starting point status + self.starting_point_ok = not self.check_starting_point + + self.joint_state_msg_received = False + + # Read all positions from parameters + self.goals = "" + for name in goal_names: + self.declare_parameter(name) + goal = self.get_parameter(name).value + if goal is None: + raise Exception(f'Values for goal "{name}" not set!') + + print(f"goal path:{goal}") + traj_msg = self.get_traj(goal) + self.trajectories.append(traj_msg) + # float_goal = [float(value) for value in goal] + # self.goals.append(float_goal) + + publish_topic = "/" + controller_name + "/" + "joint_trajectory" + self.get_logger().info( + f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ + every {wait_sec_between_publish} s' + ) + + self.get_logger().info(f"Publishing on topic:{publish_topic}") + self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) + self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) + self.i = 0 + + def timer_callback(self): + + if self.starting_point_ok: + + self.get_logger().info( + f"Sending trajectory for joints{self.trajectories[self.i].joint_names}." + ) + + self.publisher_.publish(self.trajectories[self.i]) + + self.i += 1 + self.i %= len(self.trajectories) + + elif self.check_starting_point and not self.joint_state_msg_received: + self.get_logger().warn( + 'Start configuration could not be checked! Check "joint_state" topic!' + ) + else: + self.get_logger().warn("Start configuration is not within configured limits!") + + def get_traj(self, traj_path): + with open(traj_path) as file: + file_node = yaml.load(file, Loader=yaml.FullLoader) + + traj_msg = JointTrajectory() + traj_node = file_node["trajectory"] + + traj_msg.header = Header() + traj_msg.header.frame_id = traj_node["header"]["frame_id"] + traj_msg.header.stamp.sec = traj_node["header"]["stamp"]["secs"] + traj_msg.header.stamp.nanosec = traj_node["header"]["stamp"]["nanosec"] + + for joint_name in traj_node["joint_names"]: + traj_msg.joint_names.append(joint_name) + + for i in range(len(traj_node["points"])): + point_node = traj_node["points"][i] + + point = JointTrajectoryPoint() + point.positions = point_node["positions"] + point.velocities = point_node["velocities"] + point.accelerations = point_node["accelerations"] + point.effort = point_node["effort"] + + point.time_from_start = Duration() + point.time_from_start.sec = point_node["time_from_start"]["secs"] + point.time_from_start.nanosec = point_node["time_from_start"]["nanosec"] + + traj_msg.points.append(point) + + return traj_msg + + +def main(args=None): + rclpy.init(args=args) + + publisher_forward_position = MoveLin() + + rclpy.spin(publisher_forward_position) + publisher_forward_position.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index b8be85a628..8385cf78c6 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -52,6 +52,8 @@ ros2_controllers_test_nodes.publisher_forward_position_controller:main", "publisher_joint_trajectory_controller = \ ros2_controllers_test_nodes.publisher_joint_trajectory_controller:main", + "move_lin = \ + ros2_controllers_test_nodes.move_lin:main", ], }, ) From 553dcdd09f052493e35b54667931d08683ef183b Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 30 Jun 2023 15:51:58 +0200 Subject: [PATCH 09/10] add reverting of movement --- .../ros2_controllers_test_nodes/move_lin.py | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py index 6f53393538..5319c16c16 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py @@ -73,8 +73,8 @@ def __init__(self): raise Exception(f'Values for goal "{name}" not set!') print(f"goal path:{goal}") - traj_msg = self.get_traj(goal) - self.trajectories.append(traj_msg) + self.trajectories.append(self.get_traj(goal)) + self.trajectories.append(self.get_traj_reverted(goal)) # float_goal = [float(value) for value in goal] # self.goals.append(float_goal) @@ -141,6 +141,39 @@ def get_traj(self, traj_path): return traj_msg + def get_traj_reverted(self, traj_path): + with open(traj_path) as file: + file_node = yaml.load(file, Loader=yaml.FullLoader) + + traj_msg = JointTrajectory() + traj_node = file_node["trajectory"] + + traj_msg.header = Header() + traj_msg.header.frame_id = traj_node["header"]["frame_id"] + traj_msg.header.stamp.sec = traj_node["header"]["stamp"]["secs"] + traj_msg.header.stamp.nanosec = traj_node["header"]["stamp"]["nanosec"] + + for joint_name in traj_node["joint_names"]: + traj_msg.joint_names.append(joint_name) + + for i in range(len(traj_node["points"])): + point_node = traj_node["points"][i] + point_node_revert = traj_node["points"][len(traj_node["points"]) - 1 - i] + + point = JointTrajectoryPoint() + point.positions = point_node_revert["positions"] + point.velocities = point_node_revert["velocities"] + point.accelerations = point_node_revert["accelerations"] + point.effort = point_node_revert["effort"] + + point.time_from_start = Duration() + point.time_from_start.sec = point_node["time_from_start"]["secs"] + point.time_from_start.nanosec = point_node["time_from_start"]["nanosec"] + + traj_msg.points.append(point) + + return traj_msg + def main(args=None): rclpy.init(args=args) From 8bac327f54f79dfbe763d2a4dbf2896a31417e79 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 30 Jun 2023 19:21:08 +0200 Subject: [PATCH 10/10] add duplicating --- .../ros2_controllers_test_nodes/move_lin.py | 47 ++++++++++++++----- 1 file changed, 36 insertions(+), 11 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py index 5319c16c16..3b9e4ae206 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py @@ -35,6 +35,7 @@ def __init__(self): self.declare_parameter("joints", [""]) self.declare_parameter("goal_names", ["traj1", "traj2"]) self.declare_parameter("check_starting_point", False) + self.declare_parameter("duplicate", False) # Read parameters wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value @@ -42,6 +43,7 @@ def __init__(self): controller_name = self.get_parameter("controller_name").value self.joints = self.get_parameter("joints").value self.check_starting_point = self.get_parameter("check_starting_point").value + self.duplicate = self.get_parameter("duplicate").value self.starting_point = {} self.trajectories = [] @@ -107,7 +109,9 @@ def timer_callback(self): 'Start configuration could not be checked! Check "joint_state" topic!' ) else: - self.get_logger().warn("Start configuration is not within configured limits!") + self.get_logger().warn( + "Start configuration is not within configured limits!" + ) def get_traj(self, traj_path): with open(traj_path) as file: @@ -121,17 +125,25 @@ def get_traj(self, traj_path): traj_msg.header.stamp.sec = traj_node["header"]["stamp"]["secs"] traj_msg.header.stamp.nanosec = traj_node["header"]["stamp"]["nanosec"] - for joint_name in traj_node["joint_names"]: + for joint_name in self.joints: traj_msg.joint_names.append(joint_name) for i in range(len(traj_node["points"])): point_node = traj_node["points"][i] point = JointTrajectoryPoint() - point.positions = point_node["positions"] - point.velocities = point_node["velocities"] - point.accelerations = point_node["accelerations"] - point.effort = point_node["effort"] + if self.duplicate: + point.positions = point_node["positions"] + point_node["positions"] + point.velocities = point_node["velocities"] + point_node["velocities"] + point.accelerations = ( + point_node["accelerations"] + point_node["accelerations"] + ) + point.effort = point_node["effort"] + point_node["effort"] + else: + point.positions = point_node["positions"] + point.velocities = point_node["velocities"] + point.accelerations = point_node["accelerations"] + point.effort = point_node["effort"] point.time_from_start = Duration() point.time_from_start.sec = point_node["time_from_start"]["secs"] @@ -153,7 +165,7 @@ def get_traj_reverted(self, traj_path): traj_msg.header.stamp.sec = traj_node["header"]["stamp"]["secs"] traj_msg.header.stamp.nanosec = traj_node["header"]["stamp"]["nanosec"] - for joint_name in traj_node["joint_names"]: + for joint_name in self.joints: traj_msg.joint_names.append(joint_name) for i in range(len(traj_node["points"])): @@ -161,10 +173,23 @@ def get_traj_reverted(self, traj_path): point_node_revert = traj_node["points"][len(traj_node["points"]) - 1 - i] point = JointTrajectoryPoint() - point.positions = point_node_revert["positions"] - point.velocities = point_node_revert["velocities"] - point.accelerations = point_node_revert["accelerations"] - point.effort = point_node_revert["effort"] + if self.duplicate: + point.positions = ( + point_node_revert["positions"] + point_node_revert["positions"] + ) + point.velocities = ( + point_node_revert["velocities"] + point_node_revert["velocities"] + ) + point.accelerations = ( + point_node_revert["accelerations"] + + point_node_revert["accelerations"] + ) + point.effort = point_node_revert["effort"] + point_node_revert["effort"] + else: + point.positions = point_node_revert["positions"] + point.velocities = point_node_revert["velocities"] + point.accelerations = point_node_revert["accelerations"] + point.effort = point_node_revert["effort"] point.time_from_start = Duration() point.time_from_start.sec = point_node["time_from_start"]["secs"]