From 52ee68a1c14a8ad551630aceae63af6a79554592 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Thu, 9 Jan 2025 21:56:36 -0800 Subject: [PATCH 01/11] diff_drive_controller is chainable with chained mode tests + export reference interfaces test --- 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 | 269 ++++++++++++++++-- 4 files changed, 401 insertions(+), 73 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..bbc7f97487 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 @@ -239,7 +240,9 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } -TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) +TEST_F( + TestDiffDriveController, + command_and_state_interface_configuration_succeeds_when_wheels_are_specified) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); @@ -451,14 +454,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 +516,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 +553,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 +590,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 +627,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 +786,204 @@ 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(); +} + +TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) +{ + ASSERT_EQ( + InitController(left_wheel_names, right_wheel_names), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), 2) + << "Expected exactly 2 reference interfaces: linear and angular"; + + const std::string expected_prefix_name = std::string(controller_->get_node()->get_name()); + const std::string expected_linear_interface_name = + std::string("linear/") + hardware_interface::HW_IF_VELOCITY; + const std::string expected_angular_interface_name = + std::string("angular/") + hardware_interface::HW_IF_VELOCITY; + const std::string expected_linear_name = + expected_prefix_name + std::string("/") + expected_linear_interface_name; + const std::string expected_angular_name = + expected_prefix_name + std::string("/") + expected_angular_interface_name; + + ASSERT_EQ(reference_interfaces[0]->get_name(), expected_linear_name); + ASSERT_EQ(reference_interfaces[1]->get_name(), expected_angular_name); + + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_prefix_name); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), expected_linear_interface_name); + EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_prefix_name); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), expected_angular_interface_name); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From ff3ceeb0196071bee204e012fea69bfd6be16b0a Mon Sep 17 00:00:00 2001 From: Arthur Lovekin <62527557+arthurlovekin@users.noreply.github.com> Date: Tue, 14 Jan 2025 09:37:20 -0800 Subject: [PATCH 02/11] Apply suggestions from code review small fixes: use isfinite not isnan Co-authored-by: Sai Kishor Kothakota --- diff_drive_controller/src/diff_drive_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e7c2f38b81..d8108a64da 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -153,7 +153,7 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub reference_interfaces_[1] = 0.0; } else if ( - !std::isnan(command_msg_ptr->twist.linear.x) && !std::isnan(command_msg_ptr->twist.angular.z)) + std::isfinite(command_msg_ptr->twist.linear.x) && std::isfinite(command_msg_ptr->twist.angular.z)) { reference_interfaces_[0] = command_msg_ptr->twist.linear.x; reference_interfaces_[1] = command_msg_ptr->twist.angular.z; @@ -187,7 +187,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands double linear_command = reference_interfaces_[0]; double angular_command = reference_interfaces_[1]; - if (std::isnan(linear_command) || std::isnan(angular_command)) + if (!std::isfinite(linear_command) || !std::isfinite(angular_command)) { // NaNs occur on initialization when the reference interfaces are not yet set return controller_interface::return_type::OK; @@ -721,10 +721,10 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( return controller_interface::CallbackReturn::SUCCESS; } -bool DiffDriveController::on_set_chained_mode(bool chained_mode) +bool DiffDriveController::on_set_chained_mode(bool /*chained_mode*/) { // Always accept switch to/from chained mode - return true || chained_mode; + return true; } std::vector From 4fb9bf9d64779642e2fcd00d55ff5181a239846a Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Tue, 14 Jan 2025 12:30:37 -0800 Subject: [PATCH 03/11] reset_buffers() --- .../diff_drive_controller.hpp | 3 + .../src/diff_drive_controller.cpp | 63 ++++++++----------- 2 files changed, 29 insertions(+), 37 deletions(-) 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 79d34a3a9c..922a5612e8 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 @@ -145,6 +145,9 @@ class DiffDriveController : public controller_interface::ChainableControllerInte bool reset(); void halt(); + +private: + void reset_buffers(); }; } // namespace diff_drive_controller #endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d8108a64da..3c5b63b5e4 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -36,25 +36,6 @@ 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; @@ -448,16 +429,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - 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( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -571,9 +542,6 @@ 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 = @@ -610,6 +578,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( halt(); is_halted = true; } + reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); return controller_interface::CallbackReturn::SUCCESS; @@ -622,7 +591,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( { return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.reset(); return controller_interface::CallbackReturn::SUCCESS; } @@ -640,9 +608,7 @@ bool DiffDriveController::reset() { odometry_.resetOdometry(); - // release the old queue - std::queue> empty; - std::swap(previous_two_commands_, empty); + reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); @@ -650,11 +616,34 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.reset(); is_halted = false; return true; } +void DiffDriveController::reset_buffers() +{ + reference_interfaces_ = std::vector(2, std::numeric_limits::quiet_NaN()); + // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. + std::queue> empty; + std::swap(previous_two_commands_, empty); + 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 + // 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); +} + + void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) From 5e1470bc2e6a8566146b679f58f8fed74c726231 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Tue, 14 Jan 2025 13:05:56 -0800 Subject: [PATCH 04/11] formatting --- diff_drive_controller/src/diff_drive_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 3c5b63b5e4..15eb6035d0 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -134,7 +134,8 @@ 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_ptr->twist.linear.x) && + std::isfinite(command_msg_ptr->twist.angular.z)) { reference_interfaces_[0] = command_msg_ptr->twist.linear.x; reference_interfaces_[1] = command_msg_ptr->twist.angular.z; @@ -623,13 +624,13 @@ bool DiffDriveController::reset() void DiffDriveController::reset_buffers() { reference_interfaces_ = std::vector(2, std::numeric_limits::quiet_NaN()); - // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. + // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. std::queue> empty; std::swap(previous_two_commands_, empty); 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 RealtimeBuffer 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(); @@ -643,7 +644,6 @@ void DiffDriveController::reset_buffers() received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); } - void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) From a8a45fd0d2efcaedb743719441e9fc03e2dbd399 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Tue, 14 Jan 2025 13:07:32 -0800 Subject: [PATCH 05/11] deactivate_then_activate test --- .../test/test_diff_drive_controller.cpp | 101 ++++++++++++++++++ 1 file changed, 101 insertions(+) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index bbc7f97487..d0fe0242d3 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -81,6 +81,7 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr // 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); + FRIEND_TEST(TestDiffDriveController, deactivate_then_activate); }; class TestDiffDriveController : public ::testing::Test @@ -984,6 +985,106 @@ TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) EXPECT_EQ(reference_interfaces[1]->get_interface_name(), expected_angular_interface_name); } +// Make sure that the controller is properly reset when deactivated +// and accepts new commands as expected when it is activated again. +TEST_F(TestDiffDriveController, deactivate_then_activate) +{ + 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_->set_chained_mode(false)); + + 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())); + + // published command message sets the command interfaces to the correct values + const double linear = 1.0; + 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()"; + + // Activate again + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // (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)) + << "Reference interfaces should initially be NaN on activation"; + } + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) + << "Wheels should still have the same command as when they were last set (on deactivation)"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) + << "Wheels should still have the same command as when they were last set (on deactivation)"; + + // A new command should work as expected + publish(linear, 0.0); + 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()); + + // Deactivate again and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From f27a723cc9863c7457b60fc2a32cc49547694bf2 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Tue, 14 Jan 2025 15:49:10 -0800 Subject: [PATCH 06/11] Added test command_with_zero_timestamp_is_accepted_with_warning --- .../test/test_diff_drive_controller.cpp | 51 ++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index d0fe0242d3..e3b26a718e 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -107,6 +107,11 @@ class TestDiffDriveController : public ::testing::Test * angular - the magnitude of the angular command in geometry_msgs::twist message */ void publish(double linear, double angular) + { + publish_timestamped(linear, angular, pub_node->get_clock()->now()); + } + + void publish_timestamped(double linear, double angular, rclcpp::Time timestamp) { int wait_count = 0; auto topic = velocity_publisher->get_topic_name(); @@ -122,7 +127,7 @@ class TestDiffDriveController : public ::testing::Test } geometry_msgs::msg::TwistStamped velocity_message; - velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.header.stamp = timestamp; velocity_message.twist.linear.x = linear; velocity_message.twist.angular.z = angular; velocity_publisher->publish(velocity_message); @@ -1085,6 +1090,50 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) executor.cancel(); } +TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_warning) +{ + 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_->set_chained_mode(false)); + + 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(); + + // published command message with zero timestamp sets the command interfaces to the correct values + const double linear = 1.0; + publish_timestamped(linear, 0.0, rclcpp::Time(0, 0, RCL_ROS_TIME)); + // 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()); + + // Deactivate and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From fe063fe291878b3deb9140ce637c7b2a533e81d1 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Tue, 14 Jan 2025 15:51:24 -0800 Subject: [PATCH 07/11] accidentally activated twice --- diff_drive_controller/test/test_diff_drive_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index e3b26a718e..25cf2405f2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -1054,8 +1054,6 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) // Activate again state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); waitForSetup(); From 6f3c7a5c97574d52ce5a8bfddc7ff6614f973c82 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Wed, 15 Jan 2025 11:08:50 -0800 Subject: [PATCH 08/11] Fix linting "Using C-style cast" error --- diff_drive_controller/src/diff_drive_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 238a93f076..5096b6e1fd 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -703,9 +703,10 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( return controller_interface::CallbackReturn::SUCCESS; } -bool DiffDriveController::on_set_chained_mode(bool /*chained_mode*/) +bool DiffDriveController::on_set_chained_mode(bool chained_mode) { // Always accept switch to/from chained mode + (void)chained_mode; return true; } From 794b77c14c1059ef23b7a9ebed2e63f65d7da33d Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Wed, 15 Jan 2025 13:02:42 -0800 Subject: [PATCH 09/11] og fix of linting typecast error --- diff_drive_controller/src/diff_drive_controller.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 5096b6e1fd..5dd16c471b 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -705,9 +705,8 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( bool DiffDriveController::on_set_chained_mode(bool chained_mode) { - // Always accept switch to/from chained mode - (void)chained_mode; - return true; + // Always accept switch to/from chained mode (without linting type-cast error) + return true || chained_mode; } std::vector From f795b629d2ab1b7cddc6c4797b4c0de8736869b5 Mon Sep 17 00:00:00 2001 From: Arthur Lovekin <62527557+arthurlovekin@users.noreply.github.com> Date: Mon, 20 Jan 2025 17:12:52 -0800 Subject: [PATCH 10/11] Update diff_drive_controller/src/diff_drive_controller.cpp `get_clock()->now()` to `->now()` Co-authored-by: Sai Kishor Kothakota --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 5dd16c471b..75579d6294 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -439,7 +439,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( get_node()->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); + msg->header.stamp = get_node()->now(); } const auto current_time_diff = get_node()->now() - msg->header.stamp; From f8d33e4a1a7f0c465f0e8d21385d041f9f13c138 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Mon, 20 Jan 2025 17:55:49 -0800 Subject: [PATCH 11/11] WARN_SKIPFIRST_THROTTLE for "Command message contains NaNs." --- diff_drive_controller/src/diff_drive_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 75579d6294..7356d80219 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -135,7 +135,9 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub } else { - RCLCPP_WARN(logger, "Command message contains NaNs. Not updating reference interfaces."); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, *get_node()->get_clock(), cmd_vel_timeout_.seconds() * 1000, + "Command message contains NaNs. Not updating reference interfaces."); } previous_update_timestamp_ = time;