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 265e000b79..11ecd9337b 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 std::unique_ptr limiter_linear_; std::unique_ptr limiter_angular_; @@ -139,6 +145,9 @@ class DiffDriveController : public controller_interface::ControllerInterface 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 79bdd90419..7356d80219 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -45,7 +45,7 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} +DiffDriveController::DiffDriveController() : controller_interface::ChainableControllerInterface() {} const char * DiffDriveController::feedback_type() const { @@ -97,8 +97,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) @@ -111,31 +111,64 @@ 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::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; + } + else + { + 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; + + 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::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; + } // Apply (possibly new) multipliers: const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; @@ -232,22 +265,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(); } @@ -294,7 +332,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 @@ -387,13 +425,6 @@ 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_); - // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -410,10 +441,26 @@ 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; + + 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()); } - received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) - { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -527,6 +574,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; @@ -556,9 +604,7 @@ bool DiffDriveController::reset() { odometry_.resetOdometry(); - // release the old queue - std::queue empty; - std::swap(previous_commands_, empty); + reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); @@ -566,11 +612,33 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set(nullptr); 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) @@ -636,9 +704,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 (without linting type-cast error) + 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..25cf2405f2 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,11 @@ 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); + FRIEND_TEST(TestDiffDriveController, deactivate_then_activate); }; class TestDiffDriveController : public ::testing::Test @@ -105,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(); @@ -120,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); @@ -239,7 +246,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 +460,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 +522,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 +559,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 +596,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 +633,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 +792,346 @@ 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); +} + +// 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()); + + 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(); +} + +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);