From a35362877a59d2231c3674298950bc8563c31012 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Thu, 9 Jan 2025 21:56:36 -0800 Subject: [PATCH] diff_drive_controller is chainable with chained mode tests --- diff_drive_controller/diff_drive_plugin.xml | 2 +- .../diff_drive_controller.hpp | 24 +- .../src/diff_drive_controller.cpp | 179 ++++++++++--- .../test/test_diff_drive_controller.cpp | 235 ++++++++++++++++-- 4 files changed, 368 insertions(+), 72 deletions(-) diff --git a/diff_drive_controller/diff_drive_plugin.xml b/diff_drive_controller/diff_drive_plugin.xml index 08d41cf69c..88fdfd9ecb 100644 --- a/diff_drive_controller/diff_drive_plugin.xml +++ b/diff_drive_controller/diff_drive_plugin.xml @@ -1,5 +1,5 @@ - + The differential drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot. 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 0e5e555b8c..79d34a3a9c 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 @@ -25,14 +25,14 @@ #include #include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -41,7 +41,7 @@ namespace diff_drive_controller { -class DiffDriveController : public controller_interface::ControllerInterface +class DiffDriveController : public controller_interface::ChainableControllerInterface { using TwistStamped = geometry_msgs::msg::TwistStamped; @@ -52,7 +52,11 @@ class DiffDriveController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update( + // Chainable controller replaces update() with the following two functions + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; controller_interface::CallbackReturn on_init() override; @@ -73,6 +77,10 @@ class DiffDriveController : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; protected: + bool on_set_chained_mode(bool chained_mode) override; + + std::vector on_export_reference_interfaces() override; + struct WheelHandle { std::reference_wrapper feedback; @@ -100,7 +108,7 @@ class DiffDriveController : public controller_interface::ControllerInterface Odometry odometry_; // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> @@ -114,11 +122,9 @@ class DiffDriveController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::shared_ptr last_command_msg_; - - std::queue previous_commands_; // last two commands + realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + std::queue> previous_two_commands_; // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index fd9069237c..e7c2f38b81 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -36,6 +36,25 @@ constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; } // namespace +namespace +{ // utility + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & 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(); +} + +} // namespace + namespace diff_drive_controller { using namespace std::chrono_literals; @@ -46,11 +65,11 @@ using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; DiffDriveController::DiffDriveController() -: controller_interface::ControllerInterface(), +: controller_interface::ChainableControllerInterface(), // dummy limiter, will be created in on_configure // could be done with shared_ptr instead -> but will break ABI - limiter_angular_(std::numeric_limits::quiet_NaN()), - limiter_linear_(std::numeric_limits::quiet_NaN()) + limiter_linear_(std::numeric_limits::quiet_NaN()), + limiter_angular_(std::numeric_limits::quiet_NaN()) { } @@ -104,8 +123,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons return {interface_configuration_type::INDIVIDUAL, conf_names}; } -controller_interface::return_type DiffDriveController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) +controller_interface::return_type DiffDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto logger = get_node()->get_logger(); if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) @@ -118,31 +137,61 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - // if the mutex is unable to lock, last_command_msg_ won't be updated - received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) - { last_command_msg_ = msg; }); + const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); - if (last_command_msg_ == nullptr) + if (command_msg_ptr == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg_->header.stamp; + const auto age_of_last_command = time - command_msg_ptr->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg_->twist.linear.x = 0.0; - last_command_msg_->twist.angular.z = 0.0; + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + } + else if ( + !std::isnan(command_msg_ptr->twist.linear.x) && !std::isnan(command_msg_ptr->twist.angular.z)) + { + reference_interfaces_[0] = command_msg_ptr->twist.linear.x; + reference_interfaces_[1] = command_msg_ptr->twist.angular.z; + } + else + { + RCLCPP_WARN(logger, "Command message contains NaNs. Not updating reference interfaces."); + } + + previous_update_timestamp_ = time; + + return controller_interface::return_type::OK; +} + +controller_interface::return_type DiffDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + auto logger = get_node()->get_logger(); + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg_; - double & linear_command = command.twist.linear.x; - double & angular_command = command.twist.angular.z; + double linear_command = reference_interfaces_[0]; + double angular_command = reference_interfaces_[1]; - previous_update_timestamp_ = time; + if (std::isnan(linear_command) || std::isnan(angular_command)) + { + // NaNs occur on initialization when the reference interfaces are not yet set + return controller_interface::return_type::OK; + } // Apply (possibly new) multipliers: const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; @@ -239,22 +288,27 @@ controller_interface::return_type DiffDriveController::update( } } - auto & last_command = previous_commands_.back().twist; - auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_.limit( - linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); - limiter_angular_.limit( - angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); + double & last_linear = previous_two_commands_.back()[0]; + double & second_to_last_linear = previous_two_commands_.front()[0]; + double & last_angular = previous_two_commands_.back()[1]; + double & second_to_last_angular = previous_two_commands_.front()[1]; - previous_commands_.pop(); - previous_commands_.emplace(command); + limiter_linear_.limit(linear_command, last_linear, second_to_last_linear, period.seconds()); + limiter_angular_.limit(angular_command, last_angular, second_to_last_angular, period.seconds()); + previous_two_commands_.pop(); + previous_two_commands_.push({{linear_command, angular_command}}); // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; limited_velocity_command.header.stamp = time; - limited_velocity_command.twist = command.twist; + limited_velocity_command.twist.linear.x = linear_command; + limited_velocity_command.twist.linear.y = 0.0; + limited_velocity_command.twist.linear.z = 0.0; + limited_velocity_command.twist.angular.x = 0.0; + limited_velocity_command.twist.angular.y = 0.0; + limited_velocity_command.twist.angular.z = angular_command; realtime_limited_velocity_publisher_->unlockAndPublish(); } @@ -301,7 +355,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); - cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); publish_limited_velocity_ = params_.publish_limited_velocity; // TODO(christophfroehlich) remove deprecated parameters @@ -394,12 +448,15 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - last_command_msg_ = std::make_shared(); - received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) - { stored_value = last_command_msg_; }); - // Fill last two commands with default constructed commands - previous_commands_.emplace(*last_command_msg_); - previous_commands_.emplace(*last_command_msg_); + const int nr_ref_itfs = 2; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + previous_two_commands_.push({{0.0, 0.0}}); // needs zeros (not NaN) to catch early accelerations + previous_two_commands_.push({{0.0, 0.0}}); + + std::shared_ptr empty_msg_ptr = std::make_shared(); + reset_controller_reference_msg(empty_msg_ptr, get_node()); + received_velocity_msg_ptr_.reset(); + received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( @@ -419,8 +476,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) - { stored_value = std::move(msg); }); + + const auto current_time_diff = get_node()->now() - msg->header.stamp; + + if ( + cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || + current_time_diff < cmd_vel_timeout_) + { + received_velocity_msg_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Ignoring the received message (timestamp %.10f) because it is older than " + "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)", + rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(), + cmd_vel_timeout_.seconds()); + } }); // initialize odometry publisher and message @@ -498,6 +571,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( controller_interface::CallbackReturn DiffDriveController::on_activate( const rclcpp_lifecycle::State &) { + // Set default value in command + reset_controller_reference_msg(*(received_velocity_msg_ptr_.readFromRT()), get_node()); + const auto left_result = configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_); const auto right_result = @@ -546,6 +622,7 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( { return controller_interface::CallbackReturn::ERROR; } + received_velocity_msg_ptr_.reset(); return controller_interface::CallbackReturn::SUCCESS; } @@ -564,8 +641,8 @@ bool DiffDriveController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty; - std::swap(previous_commands_, empty); + std::queue> empty; + std::swap(previous_two_commands_, empty); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); @@ -573,7 +650,7 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set(nullptr); + received_velocity_msg_ptr_.reset(); is_halted = false; return true; } @@ -643,9 +720,35 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( return controller_interface::CallbackReturn::SUCCESS; } + +bool DiffDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +std::vector +DiffDriveController::on_export_reference_interfaces() +{ + const int nr_ref_itfs = 2; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + return reference_interfaces; +} + } // namespace diff_drive_controller #include "class_loader/register_macro.hpp" CLASS_LOADER_REGISTER_CLASS( - diff_drive_controller::DiffDriveController, controller_interface::ControllerInterface) + diff_drive_controller::DiffDriveController, controller_interface::ChainableControllerInterface) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f2fc671920..396f09d226 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -47,10 +47,7 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr using DiffDriveController::DiffDriveController; std::shared_ptr getLastReceivedTwist() { - std::shared_ptr ret; - received_velocity_msg_ptr_.get( - [&ret](const std::shared_ptr & msg) { ret = msg; }); - return ret; + return *(received_velocity_msg_ptr_.readFromNonRT()); } /** @@ -80,6 +77,10 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { return realtime_odometry_publisher_; } + + // Declare these tests as friends so we can access controller_->reference_interfaces_ + FRIEND_TEST(TestDiffDriveController, chainable_controller_unchained_mode); + FRIEND_TEST(TestDiffDriveController, chainable_controller_chained_mode); }; class TestDiffDriveController : public ::testing::Test @@ -451,14 +452,28 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, test_speed_limiter) { + // If you send a linear velocity command without acceleration limits, + // then the wheel velocity command (rotations/s) will be: + // ideal_wheel_velocity_command (rotations/s) = linear_velocity_command (m/s) / wheel_radius (m). + // (The velocity command looks like a step function). + // However, if there are acceleration limits, then the actual wheel velocity command + // should always be less than the ideal velocity, and should only become + // equal at time = linear_velocity_command (m/s) / acceleration_limit (m/s^2) + + const double max_acceleration = 2.0; + const double max_deceleration = -4.0; + const double max_acceleration_reverse = -8.0; + const double max_deceleration_reverse = 10.0; ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, { - rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(2.0)), - rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(-4.0)), - rclcpp::Parameter("linear.x.max_acceleration_reverse", rclcpp::ParameterValue(-8.0)), - rclcpp::Parameter("linear.x.max_deceleration_reverse", rclcpp::ParameterValue(10.0)), + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration)), + rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(max_deceleration)), + rclcpp::Parameter( + "linear.x.max_acceleration_reverse", rclcpp::ParameterValue(max_acceleration_reverse)), + rclcpp::Parameter( + "linear.x.max_deceleration_reverse", rclcpp::ParameterValue(max_deceleration_reverse)), }), controller_interface::return_type::OK); @@ -499,17 +514,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = linear / 2.0; + const double time_acc = linear / max_acceleration; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -535,17 +551,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = -1.0 / (-4.0); + const double time_acc = -1.0 / max_deceleration; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -571,17 +588,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = -1.0 / (-8.0); + const double time_acc = -1.0 / max_acceleration_reverse; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -607,17 +625,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = 1.0 / (10.0); + const double time_acc = 1.0 / max_deceleration_reverse; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -765,6 +784,174 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) executor.cancel(); } +// When not in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 when cmd_vel_timeout_ is exceeded and on deactivation +// 3. command_interfaces are set to correct command values the command messages are not timed-out. +// In particular, make sure that the command_interface is not set to NaN right when it starts up. +TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(false)); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + // (Note: reference_interfaces_ is protected, but this is + // a FRIEND_TEST so we can use it) + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + // (these are set to 0.1 and 0.2 in InitController) + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_value())); + + // Check that a late command message causes the command interfaces to be set to 0.0 + const double linear = 1.0; + publish(linear, 0.0); + + // delay enough time to trigger the timeout (cmd_vel_timeout_ = 0.5s) + controller_->wait_for_twist(executor); + std::this_thread::sleep_for(std::chrono::milliseconds(501)); + + ASSERT_EQ( + controller_->update(pub_node->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) + << "Wheels should halt if command message is older than cmd_vel_timeout"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) + << "Wheels should halt if command message is older than cmd_vel_timeout"; + + // Now check that a timely published command message sets the command interfaces to the correct + // values + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +// When in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 on deactivation +// 3. command_interfaces are set to correct command values (not set to NaN right when it starts up) +TEST_F(TestDiffDriveController, chainable_controller_chained_mode) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(true)); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + // (these are set to 0.1 and 0.2 in InitController) + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_value())); + + // Imitate preceding controllers by setting reference_interfaces_ + // (Note: reference_interfaces_ is protected, but this is + // a FRIEND_TEST so we can use it) + const double linear = 3.0; + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);