diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index d4c4a63954..7eb0f8eea0 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -103,13 +103,13 @@ TEST_F(AckermannSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); + auto msg = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(msg.twist.linear.x)); + EXPECT_TRUE(std::isnan(msg.twist.linear.y)); + EXPECT_TRUE(std::isnan(msg.twist.linear.z)); + EXPECT_TRUE(std::isnan(msg.twist.angular.x)); + EXPECT_TRUE(std::isnan(msg.twist.angular.y)); + EXPECT_TRUE(std::isnan(msg.twist.angular.z)); } TEST_F(AckermannSteeringControllerTest, update_success) @@ -161,11 +161,11 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now(); - msg->twist.linear.x = 0.1; - msg->twist.angular.z = 0.2; - controller_->input_ref_.writeFromNonRT(msg); + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -185,7 +185,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -225,7 +225,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -249,8 +249,9 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.traction_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); - EXPECT_EQ(msg.traction_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.traction_command[STATE_TRACTION_RIGHT_WHEEL], 0.0); + EXPECT_EQ(msg.traction_command[STATE_TRACTION_LEFT_WHEEL], 0.0); EXPECT_EQ(msg.steering_angle_command[0], 2.2); EXPECT_EQ(msg.steering_angle_command[1], 4.4); diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 23aad881c6..47c9fd514a 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -31,8 +31,8 @@ #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace admittance_controller @@ -125,15 +125,16 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // admittance parameters std::shared_ptr parameter_handler_; - // ROS messages - std::shared_ptr joint_command_msg_; - - // real-time buffer - realtime_tools::RealtimeBuffer> + // real-time boxes + realtime_tools::RealtimeThreadSafeBox input_joint_command_; - realtime_tools::RealtimeBuffer input_wrench_command_; + realtime_tools::RealtimeThreadSafeBox input_wrench_command_; std::unique_ptr> state_publisher_; + // save the last commands in case of unable to get value from box + trajectory_msgs::msg::JointTrajectoryPoint joint_command_msg_; + geometry_msgs::msg::WrenchStamped wrench_command_msg_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; trajectory_msgs::msg::JointTrajectoryPoint last_reference_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5c652231fd..4f915d204c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -25,6 +25,27 @@ #include "geometry_msgs/msg/wrench.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +namespace +{ // utility + +// called from RT control loop +void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint & msg) +{ + msg.positions.clear(); + msg.velocities.clear(); +} + +// called from RT control loop +void reset_wrench_msg( + geometry_msgs::msg::WrenchStamped & msg, + const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.wrench = geometry_msgs::msg::Wrench(); +} + +} // namespace + namespace admittance_controller { @@ -133,7 +154,6 @@ AdmittanceController::on_export_reference_interfaces() reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); position_reference_ = {}; velocity_reference_ = {}; - input_wrench_command_.reset(); // assign reference interfaces auto index = 0ul; @@ -286,7 +306,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( // setup subscribers and publishers auto joint_command_callback = [this](const std::shared_ptr msg) - { input_joint_command_.writeFromNonRT(msg); }; + { input_joint_command_.set(*msg); }; input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); @@ -307,8 +327,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( msg.header.frame_id.c_str(), admittance_->parameters_.ft_sensor.frame.id.c_str()); return; } - input_wrench_command_.writeFromNonRT(msg); + input_wrench_command_.set(msg); }); + s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = @@ -390,6 +411,8 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( return controller_interface::CallbackReturn::ERROR; } } + reset_controller_reference_msg(joint_command_msg_); + reset_wrench_msg(wrench_command_msg_, get_node()); // Use current joint_state as a default reference last_reference_ = joint_state_; @@ -408,26 +431,29 @@ controller_interface::return_type AdmittanceController::update_reference_from_su { return controller_interface::return_type::ERROR; } - - joint_command_msg_ = *input_joint_command_.readFromRT(); + auto joint_command_msg_op = input_joint_command_.try_get(); + if (joint_command_msg_op.has_value()) + { + joint_command_msg_ = joint_command_msg_op.value(); + } // if message exists, load values into references - if (joint_command_msg_.get()) + if (!joint_command_msg_.positions.empty() || !joint_command_msg_.velocities.empty()) { for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { if (interface == hardware_interface::HW_IF_POSITION) { - for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + for (size_t i = 0; i < joint_command_msg_.positions.size(); ++i) { - position_reference_[i].get() = joint_command_msg_->positions[i]; + position_reference_[i].get() = joint_command_msg_.positions[i]; } } else if (interface == hardware_interface::HW_IF_VELOCITY) { - for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) + for (size_t i = 0; i < joint_command_msg_.velocities.size(); ++i) { - velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + velocity_reference_[i].get() = joint_command_msg_.velocities[i]; } } } @@ -451,7 +477,13 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); - auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench); + auto wrench_command_op = input_wrench_command_.try_get(); + if (wrench_command_op.has_value()) + { + wrench_command_msg_ = wrench_command_op.value(); + } + + auto offsetted_ft_values = add_wrenches(ft_values_, wrench_command_msg_.wrench); // apply admittance control to reference to determine desired state admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_); @@ -498,6 +530,9 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( release_interfaces(); admittance_->reset(num_joints_); + reset_controller_reference_msg(joint_command_msg_); + reset_wrench_msg(wrench_command_msg_, get_node()); + return CallbackReturn::SUCCESS; } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index a811094037..4fa5ed3251 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -89,13 +89,13 @@ TEST_F(BicycleSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); + auto msg = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(msg.twist.linear.x)); + EXPECT_TRUE(std::isnan(msg.twist.linear.y)); + EXPECT_TRUE(std::isnan(msg.twist.linear.z)); + EXPECT_TRUE(std::isnan(msg.twist.angular.x)); + EXPECT_TRUE(std::isnan(msg.twist.angular.y)); + EXPECT_TRUE(std::isnan(msg.twist.angular.z)); } TEST_F(BicycleSteeringControllerTest, update_success) @@ -147,11 +147,11 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now(); - msg->twist.linear.x = 0.1; - msg->twist.angular.z = 0.2; - controller_->input_ref_.writeFromNonRT(msg); + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -163,7 +163,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -195,7 +195,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -234,7 +234,8 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.traction_command[0], 1.1); + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.traction_command[0], 0.0); EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(0.1, 0.2); diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index c736c0032a..0ca1d023ec 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -32,8 +32,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library @@ -122,7 +122,10 @@ class DiffDriveController : public controller_interface::ChainableControllerInte bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox received_velocity_msg_; + // save the last reference in case of unable to get value from box + TwistStamped command_msg_; std::queue> previous_two_commands_; // speed limiters diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index b855c3acf1..2742c39695 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -107,15 +107,13 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub { auto logger = get_node()->get_logger(); - const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); - - if (command_msg_ptr == nullptr) + auto current_ref_op = received_velocity_msg_.try_get(); + if (current_ref_op.has_value()) { - RCLCPP_WARN(logger, "Velocity message received was a nullptr."); - return controller_interface::return_type::ERROR; + command_msg_ = current_ref_op.value(); } - const auto age_of_last_command = time - command_msg_ptr->header.stamp; + const auto age_of_last_command = time - command_msg_.header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { @@ -123,11 +121,10 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub reference_interfaces_[1] = 0.0; } else if ( - std::isfinite(command_msg_ptr->twist.linear.x) && - std::isfinite(command_msg_ptr->twist.angular.z)) + std::isfinite(command_msg_.twist.linear.x) && std::isfinite(command_msg_.twist.angular.z)) { - reference_interfaces_[0] = command_msg_ptr->twist.linear.x; - reference_interfaces_[1] = command_msg_ptr->twist.angular.z; + reference_interfaces_[0] = command_msg_.twist.linear.x; + reference_interfaces_[1] = command_msg_.twist.angular.z; } else { @@ -396,7 +393,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || current_time_diff < cmd_vel_timeout_) { - received_velocity_msg_ptr_.writeFromNonRT(msg); + received_velocity_msg_.set(*msg); } else { @@ -567,18 +564,16 @@ void DiffDriveController::reset_buffers() previous_two_commands_.push({{0.0, 0.0}}); previous_two_commands_.push({{0.0, 0.0}}); - // Fill RealtimeBuffer with NaNs so it will contain a known value + // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - received_velocity_msg_ptr_.reset(); - std::shared_ptr empty_msg_ptr = std::make_shared(); - empty_msg_ptr->header.stamp = get_node()->now(); - empty_msg_ptr->twist.linear.x = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.linear.y = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.linear.z = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.x = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.y = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.z = std::numeric_limits::quiet_NaN(); - received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); + command_msg_.header.stamp = get_node()->now(); + command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_.set(command_msg_); } void DiffDriveController::halt() diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4cd64bdd6d..bd37fcc23c 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -45,10 +45,6 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { public: using DiffDriveController::DiffDriveController; - std::shared_ptr getLastReceivedTwist() - { - return *(received_velocity_msg_ptr_.readFromNonRT()); - } /** * @brief wait_for_twist block until a new twist is received. diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 01125ac442..dda7a755db 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -111,9 +111,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -133,9 +133,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index c8b620d593..9f03042cdf 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -22,7 +22,7 @@ #include "controller_interface/controller_interface.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller @@ -86,7 +86,11 @@ class ForwardControllersBase : public controller_interface::ControllerInterface std::vector command_interface_types_; - realtime_tools::RealtimeBuffer> rt_command_ptr_; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox rt_command_; + // save the last reference in case of unable to get value from box + CmdType joint_commands_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; }; diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 331d3f9eea..6bd90c8152 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,6 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" +#include +#include #include #include #include @@ -23,12 +25,24 @@ #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" +namespace +{ // utility + +// called from RT control loop +void reset_controller_reference_msg(forward_command_controller::CmdType & msg) +{ + for (auto & data : msg.data) + { + data = std::numeric_limits::quiet_NaN(); + } +} + +} // namespace + namespace forward_command_controller { ForwardControllersBase::ForwardControllersBase() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - joints_command_subscriber_(nullptr) +: controller_interface::ControllerInterface(), joints_command_subscriber_(nullptr) { } @@ -58,7 +72,7 @@ controller_interface::CallbackReturn ForwardControllersBase::on_configure( joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + [this](const CmdType::SharedPtr msg) { rt_command_.set(*msg); }); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -101,7 +115,10 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( } // reset command buffer if a command came through callback when controller was inactive - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + reset_controller_reference_msg(joint_commands_); + rt_command_.try_set(joint_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -110,34 +127,42 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // reset command buffer - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + // Try to set default value in command. + reset_controller_reference_msg(joint_commands_); + rt_command_.try_set(joint_commands_); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type ForwardControllersBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto joint_commands = rt_command_ptr_.readFromRT(); + auto joint_commands_op = rt_command_.try_get(); + if (joint_commands_op.has_value()) + { + joint_commands_ = joint_commands_op.value(); + } // no command received yet - if (!joint_commands || !(*joint_commands)) + if (std::all_of( + joint_commands_.data.cbegin(), joint_commands_.data.cend(), + [](const auto & value) { return std::isnan(value); })) { return controller_interface::return_type::OK; } - if ((*joint_commands)->data.size() != command_interfaces_.size()) + if (joint_commands_.data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *(get_node()->get_clock()), 1000, - "command size (%zu) does not match number of interfaces (%zu)", - (*joint_commands)->data.size(), command_interfaces_.size()); + "command size (%zu) does not match number of interfaces (%zu)", joint_commands_.data.size(), + command_interfaces_.size()); return controller_interface::return_type::ERROR; } for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value((*joint_commands)->data[index]); + command_interfaces_[index].set_value(joint_commands_.data[index]); } return controller_interface::return_type::OK; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index bf20aa8eb6..3c936d8d0f 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -217,9 +218,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command_msg; + command_msg.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command_msg); // update successful, command received ASSERT_EQ( @@ -245,9 +246,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command_msg; + command_msg.data = {10.0, 20.0}; + controller_->rt_command_.set(command_msg); // update failed, command size does not match number of joints ASSERT_EQ( @@ -362,10 +363,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) ASSERT_THAT(state_if_conf.names, IsEmpty()); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); - auto command_msg = std::make_shared(); - command_msg->data = {10.0, 20.0, 30.0}; - - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + // send command + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful ASSERT_EQ( @@ -389,35 +390,31 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + auto cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // Controller is inactive but let's put some data into buffer (simulate callback when inactive) - command_msg = std::make_shared(); - command_msg->data = {5.5, 6.6, 7.7}; - - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + command.data = {5.5, 6.6, 7.7}; + controller_->rt_command_.set(command); // command ptr should be available and message should be there - same check as in `update` - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each( + ::testing::Not(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN())))); // Now activate again node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // command ptr should be reset (nullptr) after activation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after activation - same check as in `update` + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // update successful ASSERT_EQ( @@ -430,7 +427,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); // set commands again - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + controller_->rt_command_.set(command); // update successful ASSERT_EQ( 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 7bd869677c..9407118598 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 @@ -15,6 +15,7 @@ /// \authors: Jack Center, Denis Stogl #include +#include #include #include #include @@ -199,9 +200,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) SetUpController(true); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -234,9 +235,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) SetUpController(true); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( @@ -294,9 +295,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -319,35 +320,32 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes ASSERT_THAT(state_if_conf.names, IsEmpty()); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); - // command ptr should be reset (nullptr) after deactivation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after deactivation - same check as in `update` + auto cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // Controller is inactive but let's put some data into buffer (simulate callback when inactive) - auto command_msg = std::make_shared(); - command_msg->data = {5.5, 6.6, 7.7}; - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + command.data = {5.5, 6.6, 7.7}; + controller_->rt_command_.set(command); // command ptr should be available and message should be there - same check as in `update` - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each( + ::testing::Not(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN())))); // Now activate again node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // command ptr should be reset (nullptr) after activation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after activation - same check as in `update` + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // update successful ASSERT_EQ( @@ -360,7 +358,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); // set commands again - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + controller_->rt_command_.set(command); // update successful ASSERT_EQ( diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 35cf7c283d..f7d75e1c20 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -24,8 +24,8 @@ #include "controller_interface/controller_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "gpio_controllers/gpio_command_controller_parameters.hpp" @@ -91,9 +91,13 @@ class GpioCommandController : public controller_interface::ControllerInterface MapOfReferencesToCommandInterfaces command_interfaces_map_; MapOfReferencesToStateInterfaces state_interfaces_map_; - realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox rt_command_; + // save the last reference in case of unable to get value from box + CmdType gpio_commands_; + std::shared_ptr> gpio_state_publisher_{}; std::shared_ptr> realtime_gpio_state_publisher_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index fdecf71c30..bdabbb3f13 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -23,7 +23,7 @@ #include "rclcpp/subscription.hpp" namespace -{ +{ // utility template void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) { @@ -33,6 +33,15 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces } } +// called from RT control loop +void reset_controller_reference_msg( + gpio_controllers::CmdType & msg, const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.interface_groups.clear(); + msg.interface_values.clear(); +} + std::vector extract_gpios_from_hardware_info( const std::vector & hardware_infos) { @@ -85,7 +94,7 @@ try { gpios_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + [this](const CmdType::SharedPtr msg) { rt_command_.set(*msg); }); } gpio_state_publisher_ = @@ -137,14 +146,18 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State } initialize_gpio_state_msg(); - rt_command_ptr_.reset(); + // Set default value in command + reset_controller_reference_msg(gpio_commands_, get_node()); + rt_command_.try_set(gpio_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; } CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { - rt_command_ptr_.reset(); + // Set default value in command + reset_controller_reference_msg(gpio_commands_, get_node()); + rt_command_.try_set(gpio_commands_); return CallbackReturn::SUCCESS; } @@ -315,19 +328,24 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( controller_interface::return_type GpioCommandController::update_gpios_commands() { - auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); - if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + auto gpio_commands_op = rt_command_.try_get(); + if (gpio_commands_op.has_value()) { + gpio_commands_ = gpio_commands_op.value(); + } + if (gpio_commands_.interface_groups.empty() || gpio_commands_.interface_values.empty()) + { + // no command received yet return controller_interface::return_type::OK; } - const auto gpio_commands = *(*gpio_commands_ptr); - for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + for (std::size_t gpio_index = 0; gpio_index < gpio_commands_.interface_groups.size(); + ++gpio_index) { - const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + const auto & gpio_name = gpio_commands_.interface_groups[gpio_index]; if ( - gpio_commands.interface_values[gpio_index].values.size() != - gpio_commands.interface_values[gpio_index].interface_names.size()) + gpio_commands_.interface_values[gpio_index].values.size() != + gpio_commands_.interface_values[gpio_index].interface_names.size()) { RCLCPP_ERROR( get_node()->get_logger(), "For gpio %s interfaces_names do not match values", @@ -335,10 +353,10 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() return controller_interface::return_type::ERROR; } for (std::size_t command_interface_index = 0; - command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + command_interface_index < gpio_commands_.interface_values[gpio_index].values.size(); ++command_interface_index) { - apply_command(gpio_commands, gpio_index, command_interface_index); + apply_command(gpio_commands_, gpio_index, command_interface_index); } } return controller_interface::return_type::OK; diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index de085894f2..c252ed4bf9 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -445,7 +445,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); @@ -467,7 +467,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); @@ -489,7 +489,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -513,7 +513,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -536,8 +536,7 @@ TEST_F( const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); - - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -561,8 +560,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), createInterfaceValue({"ana.1"}, {21.0})}); - - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 189805c17c..c0edc8c5a2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_srvs/srv/set_bool.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -109,14 +109,15 @@ class MecanumDriveController : public controller_interface::ChainableControllerI */ std::vector state_joint_names_; - // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. - // used for preceding controller - std::vector reference_names_; + // the RT Box containing the command message + realtime_tools::RealtimeThreadSafeBox input_ref_; + // save the last reference in case of unable to get value from box + ControllerReferenceMsg current_ref_; + // the reference timeout value from parameters + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // Command subscribers and Controller State, odom state, tf state publishers rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; - rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); using OdomStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr odom_s_publisher_; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 79a1b54846..823764ccfe 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -15,9 +15,10 @@ #ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ -#include "geometry_msgs/msg/twist.hpp" -#include "realtime_tools/realtime_buffer.hpp" -#include "realtime_tools/realtime_publisher.hpp" +#include +#include + +#include "rclcpp/time.hpp" #define PLANAR_POINT_DIM 3 diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 9615d3e0c8..793043f990 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -33,16 +33,15 @@ using ControllerReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) + ControllerReferenceMsg & msg, const std::shared_ptr & node) { - msg->header.stamp = node->now(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = std::numeric_limits::quiet_NaN(); + msg.header.stamp = node->now(); + msg.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } } // namespace @@ -129,9 +128,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( "~/reference", subscribers_qos, std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); try { @@ -252,7 +250,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptrheader.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); - reset_controller_reference_msg(msg, get_node()); + ControllerReferenceMsg emtpy_msg; + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.set(emtpy_msg); } } @@ -324,8 +324,11 @@ bool MecanumDriveController::on_set_chained_mode(bool /*chained_mode*/) { return controller_interface::CallbackReturn MecanumDriveController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + ControllerReferenceMsg emtpy_msg; + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.try_set(emtpy_msg); return controller_interface::CallbackReturn::SUCCESS; } @@ -354,41 +357,50 @@ controller_interface::CallbackReturn MecanumDriveController::on_deactivate( controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; + auto current_ref_op = input_ref_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - current_ref_.header.stamp; - // send message only if there is no timeout + // accept message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { if ( - !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) + !std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y) && + !std::isnan(current_ref_.twist.angular.z)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.linear.y; - reference_interfaces_[2] = current_ref->twist.angular.z; + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = current_ref_.twist.linear.y; + reference_interfaces_[2] = current_ref_.twist.angular.z; if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref_); } } } else { if ( - !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) + !std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y) && + !std::isnan(current_ref_.twist.angular.z)) { reference_interfaces_[0] = 0.0; reference_interfaces_[1] = 0.0; reference_interfaces_[2] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref_); } } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 31c6c36737..ae7f551693 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -286,10 +286,9 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); - - ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); + auto msg = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(msg.twist.linear.x)); + ASSERT_TRUE(std::isnan(msg.twist.angular.z)); } TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) @@ -407,11 +406,11 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = *(controller_->input_ref_.readFromNonRT()); - auto old_timestamp = reference->header.stamp; - EXPECT_TRUE(std::isnan(reference->twist.linear.x)); - EXPECT_TRUE(std::isnan(reference->twist.linear.y)); - EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + auto old_timestamp = reference.header.stamp; + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -420,10 +419,10 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); controller_->wait_for_commands(executor); - ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); + ASSERT_EQ(old_timestamp, reference.header.stamp); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); } // when time stamp is zero expect that time stamp is set to current time stamp @@ -438,11 +437,11 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + auto old_timestamp = reference.header.stamp; + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -450,13 +449,15 @@ TEST_F( publish_commands(rclcpp::Time(0)); controller_->wait_for_commands(executor); - ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); - EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); + reference = controller_->input_ref_.get(); + + ASSERT_EQ(old_timestamp.sec, reference.header.stamp.sec); + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); + EXPECT_NE(reference.header.stamp.sec, 0.0); } // when the reference_msg has valid timestamp then the timeout check in reference_callback() @@ -471,9 +472,9 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -481,11 +482,12 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer publish_commands(controller_->get_node()->now()); controller_->wait_for_commands(executor); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + reference = controller_->input_ref_.get(); + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); } // when not in chainable mode and ref_msg_timedout expect @@ -514,23 +516,24 @@ TEST_F( // set command statically joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg, msg_2; + + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + auto reference = controller_->input_ref_.get(); + const auto age_of_last_command = controller_->get_node()->now() - reference.header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -546,21 +549,21 @@ TEST_F( EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + msg_2.header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2.twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg_2); + + reference = controller_->input_ref_.get(); + const auto age_of_last_command_2 = controller_->get_node()->now() - reference.header.stamp; // age_of_last_command_2 < ref_timeout_ ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -574,7 +577,7 @@ TEST_F( // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * // 0.0) EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -661,21 +664,22 @@ TEST_F( joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); - std::shared_ptr msg = std::make_shared(); - - msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + ControllerReferenceMsg msg; + + msg.header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + auto reference = controller_->input_ref_.get(); + + const auto age_of_last_command = controller_->get_node()->now() - reference.header.stamp; ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -689,7 +693,8 @@ TEST_F( // velocity_in_center_frame_angular_z_); // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); - ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + reference = controller_->input_ref_.get(); + ASSERT_TRUE(std::isnan(reference.twist.linear.x)); } TEST_F( @@ -702,9 +707,10 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // reference_callback() is called implicitly when publish_commands() is called. @@ -713,13 +719,14 @@ TEST_F( publish_commands(controller_->get_node()->now()); controller_->wait_for_commands(executor); - - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + reference = controller_->input_ref_.get(); + + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.linear.y)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); } TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 0bd48b65f5..93a6f6ec65 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -36,8 +36,8 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // Project #include "parallel_gripper_controller/parallel_gripper_action_controller_parameters.hpp" @@ -93,10 +93,6 @@ class GripperActionController : public controller_interface::ControllerInterface controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is reused to set the realtime buffer - Commands command_struct_, command_struct_rt_; - protected: using GripperCommandAction = control_msgs::action::ParallelGripperCommand; using ActionServer = rclcpp_action::Server; @@ -105,12 +101,16 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + using RealtimeGoalHandleBox = realtime_tools::RealtimeThreadSafeBox; - bool update_hold_position_; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox command_; + // pre-allocated memory that is reused + Commands command_struct_; + // save the last reference in case of unable to get value from box + Commands command_struct_rt_; - bool verbose_ = false; ///< Hard coded verbose flag to help in debugging - std::string name_; ///< Controller name. + std::string name_; ///< Controller name. std::optional> joint_command_interface_; std::optional> @@ -125,7 +125,7 @@ class GripperActionController : public controller_interface::ControllerInterface std::shared_ptr param_listener_; Params params_; - RealtimeGoalHandleBuffer + RealtimeGoalHandleBox rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 50f91678cb..87bd2c34a5 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -30,12 +30,14 @@ namespace parallel_gripper_action_controller void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (active_goal) { // Marks the current goal as canceled active_goal->setCanceled(std::make_shared()); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } } @@ -57,7 +59,11 @@ controller_interface::CallbackReturn GripperActionController::on_init() controller_interface::return_type GripperActionController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - command_struct_rt_ = *(command_.readFromRT()); + auto command_struct_rt_op = command_.try_get(); + if (command_struct_rt_op.has_value()) + { + command_struct_rt_ = command_struct_rt_op.value(); + } const double current_position = joint_position_state_interface_->get().get_value(); const double current_velocity = joint_velocity_state_interface_->get().get_value(); @@ -124,14 +130,14 @@ void GripperActionController::accepted_callback( { command_struct_.max_effort_ = params_.max_effort; } - command_.writeFromNonRT(command_struct_); + command_.set(command_struct_); pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; last_movement_time_ = get_node()->now(); rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); + rt_active_goal_.set([rt_goal](RealtimeGoalHandlePtr & stored_value) { stored_value = rt_goal; }); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -148,7 +154,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode @@ -161,7 +168,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( auto action_res = std::make_shared(); active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -171,14 +179,15 @@ void GripperActionController::set_hold_position() command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; - command_.writeFromNonRT(command_struct_); + command_.set(command_struct_); } void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (!active_goal) { return; @@ -192,7 +201,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); active_goal->setSucceeded(pre_alloc_result_); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } else { @@ -217,7 +227,8 @@ void GripperActionController::check_for_success( RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } } } @@ -316,11 +327,11 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Command - non RT version + // Command command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; - command_.initRT(command_struct_); + command_.try_set(command_struct_); // Result pre_alloc_result_ = std::make_shared(); diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index c60dd3eac1..bc3f823dfd 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -18,6 +18,7 @@ #ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ #define PID_CONTROLLER__PID_CONTROLLER_HPP_ +#include #include #include #include @@ -27,8 +28,8 @@ #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_srvs/srv/set_bool.hpp" #include "pid_controller/pid_controller_parameters.hpp" @@ -80,10 +81,12 @@ class PidController : public controller_interface::ChainableControllerInterface // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeThreadSafeBox input_ref_; + ControllerReferenceMsg current_ref_; rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> measured_state_; + realtime_tools::RealtimeThreadSafeBox measured_state_; + ControllerMeasuredStateMsg current_state_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c32c216a38..c49cffff01 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -53,15 +53,15 @@ using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceM // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, const std::vector & dof_names) + ControllerCommandMsg & msg, const std::vector & dof_names) { - msg->dof_names = dof_names; - msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); - msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg.dof_names = dof_names; + msg.values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg.values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); } void reset_controller_measured_state_msg( - const std::shared_ptr & msg, const std::vector & dof_names) + ControllerCommandMsg & msg, const std::vector & dof_names) { reset_controller_reference_msg(msg, dof_names); } @@ -175,9 +175,8 @@ controller_interface::CallbackReturn PidController::on_configure( "~/reference", subscribers_qos, std::bind(&PidController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, reference_and_state_dof_names_); - input_ref_.writeFromNonRT(msg); + reset_controller_reference_msg(current_ref_, reference_and_state_dof_names_); + input_ref_.set(current_ref_); // input state Subscriber and callback if (params_.use_external_measured_states) @@ -223,16 +222,15 @@ controller_interface::CallbackReturn PidController::on_configure( } } // TODO(destogl): Sort the input values based on joint and interface names - measured_state_.writeFromNonRT(state_msg); + measured_state_.set(*state_msg); }; measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); } - std::shared_ptr measured_state_msg = - std::make_shared(); + ControllerMeasuredStateMsg measured_state_msg; reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); - measured_state_.writeFromNonRT(measured_state_msg); + measured_state_.set(measured_state_msg); measured_state_values_.resize( dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); @@ -275,7 +273,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names = reference_and_state_dof_names_; - input_ref_.writeFromNonRT(ref_msg); + input_ref_.set(*ref_msg); } else if ( msg->dof_names.size() == reference_and_state_dof_names_.size() && @@ -284,7 +282,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.size(); ++i) @@ -307,7 +305,7 @@ void PidController::reference_callback(const std::shared_ptr::quiet_NaN()); @@ -431,21 +439,26 @@ controller_interface::CallbackReturn PidController::on_activate( controller_interface::return_type PidController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto current_ref = input_ref_.readFromRT(); + auto current_ref_op = input_ref_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_ = current_ref_op.value(); + } for (size_t i = 0; i < dof_; ++i) { - if (!std::isnan((*current_ref)->values[i])) + if (!std::isnan(current_ref_.values[i])) { - reference_interfaces_[i] = (*current_ref)->values[i]; - if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + reference_interfaces_[i] = current_ref_.values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan(current_ref_.values_dot[i])) { - reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + reference_interfaces_[dof_ + i] = current_ref_.values_dot[i]; } - - (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + current_ref_.values[i] = std::numeric_limits::quiet_NaN(); } } + // save cleared input_ref_ + input_ref_.try_set(current_ref_); return controller_interface::return_type::OK; } @@ -458,13 +471,17 @@ controller_interface::return_type PidController::update_and_write_commands( // Update feedback either from external measured state or from state interfaces if (params_.use_external_measured_states) { - const auto measured_state = *(measured_state_.readFromRT()); + auto measured_state_op = measured_state_.try_get(); + if (measured_state_op.has_value()) + { + current_state_ = measured_state_op.value(); + } for (size_t i = 0; i < dof_; ++i) { - measured_state_values_[i] = measured_state->values[i]; + measured_state_values_[i] = current_state_.values[i]; if (measured_state_values_.size() == 2 * dof_) { - measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + measured_state_values_[dof_ + i] = current_state_.values_dot[i]; } } } @@ -490,9 +507,9 @@ controller_interface::return_type PidController::update_and_write_commands( if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) { // calculate feed-forward - // two interfaces if (reference_interfaces_.size() == 2 * dof_) { + // two interfaces if (std::isfinite(reference_interfaces_[dof_ + i])) { tmp_command = reference_interfaces_[dof_ + i] * diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 46d214f616..8aa4abe2cf 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -131,14 +131,14 @@ TEST_F(PidControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_EQ((*msg)->values.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->values) + auto msg = controller_->input_ref_.get(); + EXPECT_EQ(msg.values.size(), dof_names_.size()); + for (const auto & cmd : msg.values) { EXPECT_TRUE(std::isnan(cmd)); } - EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->values_dot) + EXPECT_EQ(msg.values_dot.size(), dof_names_.size()); + for (const auto & cmd : msg.values_dot) { EXPECT_TRUE(std::isnan(cmd)); } @@ -209,7 +209,7 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain) controller_->set_chained_mode(false); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -219,8 +219,8 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain) for (size_t i = 0; i < dof_command_values_.size(); ++i) { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().values[i])); + EXPECT_EQ(controller_->input_ref_.get().values[i], dof_command_values_[i]); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); } @@ -233,7 +233,7 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain) EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); for (size_t i = 0; i < dof_command_values_.size(); ++i) { - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[i])); // check the command value: // ref = 101.101, state = 1.1, ds = 0.01 diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 39385c5093..97180a389c 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -106,15 +106,15 @@ class TestablePidController : public pid_controller::PidController void set_reference(const std::vector & target_value) { - std::shared_ptr msg = std::make_shared(); - msg->dof_names = params_.dof_names; - msg->values.resize(msg->dof_names.size(), 0.0); - for (size_t i = 0; i < msg->dof_names.size(); ++i) + ControllerCommandMsg msg; + msg.dof_names = params_.dof_names; + msg.values.resize(msg.dof_names.size(), 0.0); + for (size_t i = 0; i < msg.dof_names.size(); ++i) { - msg->values[i] = target_value[i]; + msg.values[i] = target_value[i]; } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - input_ref_.writeFromNonRT(msg); + msg.values_dot.resize(msg.dof_names.size(), std::numeric_limits::quiet_NaN()); + input_ref_.set(msg); } }; @@ -137,7 +137,8 @@ class PidControllerFixture : public ::testing::Test // initialize controller controller_ = std::make_unique(); - command_publisher_node_ = std::make_shared("command_publisher"); + auto test = std::make_shared("command_publisher"); + command_publisher_node_ = test; command_publisher_ = command_publisher_node_->create_publisher( "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); } diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index ea7005c543..7d575e7d6a 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -111,9 +111,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -133,9 +133,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e700767e4c..83057652a5 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // TODO(anyone): Replace with controller specific messages #include "control_msgs/msg/steering_controller_status.hpp" @@ -81,11 +81,14 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::shared_ptr param_listener_; steering_controllers_library::Params params_; - // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + // the RT Box containing the command message + realtime_tools::RealtimeThreadSafeBox input_ref_; + // save the last reference in case of unable to get value from box + ControllerTwistReferenceMsg current_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index cff2229380..2a117a171d 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -31,16 +31,15 @@ using ControllerTwistReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) + ControllerTwistReferenceMsg & msg, const std::shared_ptr & node) { - msg->header.stamp = node->now(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = std::numeric_limits::quiet_NaN(); + msg.header.stamp = node->now(); + msg.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } } // namespace @@ -258,10 +257,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = - std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); try { @@ -358,7 +355,7 @@ void SteeringControllersLibrary::reference_callback( if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { - input_ref_.writeFromNonRT(msg); + input_ref_.set(*msg); } else { @@ -443,8 +440,10 @@ bool SteeringControllersLibrary::on_set_chained_mode(bool /*chained_mode*/) { re controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.try_set(current_ref_); return controller_interface::CallbackReturn::SUCCESS; } @@ -460,14 +459,45 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( } controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); + auto current_ref_op = input_ref_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - current_ref_.header.stamp; + + // accept message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y)) + { + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = current_ref_.twist.angular.z; - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref_); + } + } + } + else { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.angular.z)) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref_); + } } return controller_interface::return_type::OK; @@ -485,27 +515,26 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; - const auto timeout = - age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); - - // store (for open loop odometry) and set commands - last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; - last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; - auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + command_interfaces_[i].set_value(traction_commands[i]); } for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { command_interfaces_[i + params_.traction_joints_names.size()].set_value(steering_commands[i]); } } + else + { + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) + { + command_interfaces_[i].set_value(0.0); + } + } // Publish odometry message // Compute and store orientation info diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 26750777c9..a88b84874c 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -80,11 +80,12 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) } // Tests controller update_reference_from_subscribers and -// its two cases for position_feedback true/false behavior +// for position_feedback behavior // when too old msg is sent i.e age_of_last_command > ref_timeout case -TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) { SetUpController(); + controller_->params_.position_feedback = true; rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -104,46 +105,76 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg; - // adjusting to achieve age_of_last_command > ref_timeout - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // case 1 position_feedback = false - controller_->params_.position_feedback = false; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); - // age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + // age_of_last_command < ref_timeout_ + auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // are the command_itfs updated? + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + + // adjusting to achieve age_of_last_command > ref_timeout + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // adjusting to achieve age_of_last_command > ref_timeout + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // Wheel velocities should reset to 0 EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); @@ -152,42 +183,94 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // Steer angles should not reset EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); +} - // case 2 position_feedback = true - controller_->params_.position_feedback = true; +// Tests controller update_reference_from_subscribers and +// for position_feedback=false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) +{ + SetUpController(); + controller_->params_.position_feedback = false; - // adjusting to achieve age_of_last_command > ref_timeout - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); - // age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const double TEST_LINEAR_VELOCITY_X = 1.5; + const double TEST_LINEAR_VELOCITY_Y = 0.0; + const double TEST_ANGULAR_VELOCITY_Z = 0.3; + + ControllerReferenceMsg msg; + + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // age_of_last_command < ref_timeout_ + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // are the command_itfs updated? + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + + // adjusting to achieve age_of_last_command > ref_timeout + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // Wheel velocities should reset to 0 EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 22d70fd7a5..5aac580b33 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -70,7 +70,8 @@ class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); - FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout); + FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout); public: controller_interface::CallbackReturn on_configure( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 55c688cd5d..2803c1394e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -96,13 +96,13 @@ TEST_F(TricycleSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); - EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); + auto msg = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(msg.twist.linear.x)); + EXPECT_TRUE(std::isnan(msg.twist.linear.y)); + EXPECT_TRUE(std::isnan(msg.twist.linear.z)); + EXPECT_TRUE(std::isnan(msg.twist.angular.x)); + EXPECT_TRUE(std::isnan(msg.twist.angular.y)); + EXPECT_TRUE(std::isnan(msg.twist.angular.z)); } TEST_F(TricycleSteeringControllerTest, update_success) @@ -154,11 +154,11 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now(); - msg->twist.linear.x = 0.1; - msg->twist.angular.z = 0.2; - controller_->input_ref_.writeFromNonRT(msg); + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -174,7 +174,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -211,7 +211,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -235,8 +235,9 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.traction_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); - EXPECT_EQ(msg.traction_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.traction_command[STATE_TRACTION_RIGHT_WHEEL], 0.0); + EXPECT_EQ(msg.traction_command[STATE_TRACTION_LEFT_WHEEL], 0.0); EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 9a9af3fb81..fd7dec4cb2 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -111,9 +111,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -133,9 +133,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ(