diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 326f05a579..f2c43acbe0 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -59,7 +59,8 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop( + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input); } else { diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 896163ef91..4dc6d1cb0e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -103,7 +103,7 @@ TEST_F(AckermannSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.get(); + auto msg = controller_->input_ref_twist_.get(); EXPECT_TRUE(std::isnan(msg.twist.linear.x)); EXPECT_TRUE(std::isnan(msg.twist.linear.y)); EXPECT_TRUE(std::isnan(msg.twist.linear.z)); @@ -165,7 +165,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) msg.header.stamp = controller_->get_node()->now(); msg.twist.linear.x = 0.1; msg.twist.angular.z = 0.2; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -185,7 +185,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -225,7 +225,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index b10ec445b7..b8a370c9df 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -49,7 +49,8 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop( + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input); } else { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index b6009e1d7a..df165213b7 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -89,7 +89,7 @@ TEST_F(BicycleSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.get(); + auto msg = controller_->input_ref_twist_.get(); EXPECT_TRUE(std::isnan(msg.twist.linear.x)); EXPECT_TRUE(std::isnan(msg.twist.linear.y)); EXPECT_TRUE(std::isnan(msg.twist.linear.z)); @@ -151,7 +151,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) msg.header.stamp = controller_->get_node()->now(); msg.twist.linear.x = 0.1; msg.twist.angular.z = 0.2; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -164,7 +164,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -197,7 +197,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index d57b00d278..47eeaa2308 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -72,9 +72,14 @@ if(BUILD_TESTING) target_include_directories(test_steering_controllers_library PRIVATE include) target_link_libraries(test_steering_controllers_library steering_controllers_library) + add_rostest_with_parameters_gmock( + test_steering_controllers_library_steering_input test/test_steering_controllers_library_steering_input.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_steering_input_params.yaml) + target_include_directories(test_steering_controllers_library_steering_input PRIVATE include) + target_link_libraries(test_steering_controllers_library_steering_input steering_controllers_library) + ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) target_link_libraries(test_steering_odometry steering_controllers_library) - endif() install( diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 1aac29d543..16f84cded4 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -6,6 +6,7 @@ steering_controllers_library ============================= .. _steering_controller_status_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerStatus.msg +.. _steering_controller_command_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerCommand.msg .. _odometry_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/nav_msgs/msg/Odometry.msg .. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg .. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg @@ -21,7 +22,7 @@ For an introduction to mobile robot kinematics and the nomenclature used here, s Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used. +The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used if ``twist_input == true``. If ``twist_input == false``, the controller uses `control_msgs/msg/SteeringControllerStatus `_ where linear ``speed`` and angular ``steering_angle`` components are used. Values in other components are ignored. In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. @@ -82,9 +83,16 @@ With the following state interfaces: Subscribers ,,,,,,,,,,,, -Used when controller is not in chained mode (``in_chained_mode == false``). - +Used when controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is activated (``twist_input == true``): - ``/reference`` [`geometry_msgs/msg/TwistStamped `_] +In this configuration the controller uses : +- **Linear Velocity (`linear`)**: Represents the linear speed of the robot (in meters per second, m/s). +- **Angular Velocity (`angular`)**: Represents the angular speed of the robot (in meters per second, m/s). +When the controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is not activated (``twist_input == false``): +- ``/reference`` [`control_msgs/msg/SteeringControllerCommand `_] +In this configuration the controller uses : +- **Linear Velocity (`speed`)**: Represents the linear velocity of the robot (in meters per second, m/s). +- **Steering angle (`steering_angle`)**: Represents the angle of the imaginary, central steering wheel relative to the vehicle’s longitudinal axis. Specific angles for individual steering joints are computed internally based on the kinematic model of the robot. (in radians, rad) Publishers ,,,,,,,,,,, diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 6deabe8237..3694b8dc16 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -22,11 +22,13 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_thread_safe_box.hpp" // TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/steering_controller_command.hpp" #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -70,6 +72,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp::Time & time, const rclcpp::Duration & period) override; using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerSteeringReferenceMsg = control_msgs::msg::SteeringControllerCommand; using ControllerStateMsgOdom = nav_msgs::msg::Odometry; using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; using SteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus; @@ -81,14 +84,17 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::shared_ptr param_listener_; steering_controllers_library::Params params_; - // the RT Box containing the command message - realtime_tools::RealtimeThreadSafeBox input_ref_; - // save the last reference in case of unable to get value from box - ControllerTwistReferenceMsg current_ref_; - rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms - // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_steering_ = + nullptr; + // the RT Box containing the command messages + realtime_tools::RealtimeThreadSafeBox input_ref_twist_; + realtime_tools::RealtimeThreadSafeBox input_ref_steering_; + // save the last references in case of unable to get value from box + ControllerTwistReferenceMsg current_ref_twist_; + ControllerSteeringReferenceMsg current_ref_steering_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; @@ -131,7 +137,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl private: // callback for topic interface - void reference_callback(const std::shared_ptr msg); + void reference_callback_twist(const std::shared_ptr msg); + void reference_callback_steering(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 7b243e795c..502eeaf09e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -136,8 +136,10 @@ class SteeringOdometry * \param v_bx Linear velocity [m/s] * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call + * \param use_twist_input If true, the input is twist, otherwise it is steering angle */ - void update_open_loop(const double v_bx, const double omega_bz, const double dt); + void update_open_loop( + const double v_bx, const double omega_bz, const double dt, const bool use_twist_input = true); /** * \brief Set odometry type @@ -206,12 +208,14 @@ class SteeringOdometry * \param omega_bz Desired angular velocity of the robot around x_z-axis * \param open_loop If false, the IK will be calculated using measured steering angle * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * \param use_twist_input If true, the input is twist, otherwise it is steering angle * has been reached * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( const double v_bx, const double omega_bz, const bool open_loop = true, - const bool reduce_wheel_speed_until_steering_reached = false); + const bool reduce_wheel_speed_until_steering_reached = false, + const bool use_twist_input = true); /** * \brief Reset poses, heading, and accumulators @@ -250,6 +254,13 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates angular velocity from the steering angle and linear velocity + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param phi Steering angle + */ + double convert_steering_angle_to_angular_velocity(double v_bx, double phi); + /** * \brief Calculates linear velocity of a robot with double traction axle * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index dd57971f30..4c49e314a5 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -28,6 +28,8 @@ namespace using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using ControllerSteeringReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg; // called from RT control loop void reset_controller_reference_msg( @@ -42,6 +44,15 @@ void reset_controller_reference_msg( msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } +void reset_controller_reference_msg( + ControllerSteeringReferenceMsg & msg, + const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.linear_velocity = std::numeric_limits::quiet_NaN(); + msg.steering_angle = std::numeric_limits::quiet_NaN(); +} + } // namespace namespace steering_controllers_library @@ -253,12 +264,25 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - ref_subscriber_twist_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - reset_controller_reference_msg(current_ref_, get_node()); - input_ref_.set(current_ref_); + if (params_.use_twist_input) + { + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_twist, this, std::placeholders::_1)); + reset_controller_reference_msg(current_ref_twist_, get_node()); + input_ref_twist_.set(current_ref_twist_); + } + else + { + ref_subscriber_steering_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_steering, this, std::placeholders::_1)); + reset_controller_reference_msg(current_ref_steering_, get_node()); + input_ref_steering_.set(current_ref_steering_); + } try { @@ -334,7 +358,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void SteeringControllersLibrary::reference_callback( +void SteeringControllersLibrary::reference_callback_twist( const std::shared_ptr msg) { // if no timestamp provided use current time for command timestamp @@ -349,7 +373,35 @@ void SteeringControllersLibrary::reference_callback( if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { - input_ref_.set(*msg); + input_ref_twist_.set(*msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +void SteeringControllersLibrary::reference_callback_steering( + const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_steering_.set(*msg); } else { @@ -423,7 +475,9 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.push_back( hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/angular"), + (params_.use_twist_input ? hardware_interface::HW_IF_VELOCITY + : hardware_interface::HW_IF_POSITION), &reference_interfaces_[1])); return reference_interfaces; @@ -436,8 +490,16 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( { // Try to set default value in command. // If this fails, then another command will be received soon anyways. - reset_controller_reference_msg(current_ref_, get_node()); - input_ref_.try_set(current_ref_); + if (params_.use_twist_input) + { + reset_controller_reference_msg(current_ref_twist_, get_node()); + input_ref_twist_.try_set(current_ref_twist_); + } + else + { + reset_controller_reference_msg(current_ref_steering_, get_node()); + input_ref_steering_.try_set(current_ref_steering_); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -462,42 +524,94 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref_op = input_ref_.try_get(); - if (current_ref_op.has_value()) + if (params_.use_twist_input) { - current_ref_ = current_ref_op.value(); - } + auto current_ref_op = input_ref_twist_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_twist_ = current_ref_op.value(); + } - const auto age_of_last_command = time - current_ref_.header.stamp; + const auto age_of_last_command = time - current_ref_twist_.header.stamp; - // accept message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) - { - if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y)) + // accept message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - reference_interfaces_[0] = current_ref_.twist.linear.x; - reference_interfaces_[1] = current_ref_.twist.angular.z; + if ( + !std::isnan(current_ref_twist_.twist.linear.x) && + !std::isnan(current_ref_twist_.twist.angular.z)) + { + reference_interfaces_[0] = current_ref_twist_.twist.linear.x; + reference_interfaces_[1] = current_ref_twist_.twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref_twist_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_twist_.twist.angular.z = std::numeric_limits::quiet_NaN(); - if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + input_ref_twist_.try_set(current_ref_twist_); + } + } + } + else + { + if ( + !std::isnan(current_ref_twist_.twist.linear.x) && + !std::isnan(current_ref_twist_.twist.angular.z)) { - current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - input_ref_.try_set(current_ref_); + current_ref_twist_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_twist_.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_twist_.try_set(current_ref_twist_); } } } else { - if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.angular.z)) + auto current_ref_op = input_ref_steering_.try_get(); + if (current_ref_op.has_value()) { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + current_ref_steering_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - current_ref_steering_.header.stamp; - current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + // accept message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if ( + !std::isnan(current_ref_steering_.linear_velocity) && + !std::isnan(current_ref_steering_.steering_angle)) + { + reference_interfaces_[0] = current_ref_steering_.linear_velocity; + reference_interfaces_[1] = current_ref_steering_.steering_angle; - input_ref_.try_set(current_ref_); + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref_steering_.linear_velocity = std::numeric_limits::quiet_NaN(); + current_ref_steering_.steering_angle = std::numeric_limits::quiet_NaN(); + + input_ref_steering_.try_set(current_ref_steering_); + } + } + } + else + { + if ( + !std::isnan(current_ref_steering_.linear_velocity) && + !std::isnan(current_ref_steering_.steering_angle)) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + current_ref_steering_.linear_velocity = std::numeric_limits::quiet_NaN(); + current_ref_steering_.steering_angle = std::numeric_limits::quiet_NaN(); + + input_ref_steering_.try_set(current_ref_steering_); + } } } @@ -517,9 +631,16 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { + const auto age_of_last_command = params_.use_twist_input + ? time - current_ref_twist_.header.stamp + : time - current_ref_steering_.header.stamp; + + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, - params_.reduce_wheel_speed_until_steering_reached); + params_.reduce_wheel_speed_until_steering_reached, params_.use_twist_input); for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 809e564377..678d7b985d 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -114,3 +114,10 @@ steering_controllers_library: position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } + + use_twist_input: { + type: bool, + default_value: true, + description: "Choose whether a TwistStamped or a SteeringControllerCommand message is used as input.", + read_only: true, + } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0a5367f4bd..92b49fa2e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -126,7 +126,8 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_; + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos); return update_odometry(linear_velocity, angular_velocity, dt); } @@ -160,7 +161,8 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheel_base_; + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } @@ -180,19 +182,24 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheel_base_; + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) +void SteeringOdometry::update_open_loop( + const double v_bx, const double last_angular_command_, const double dt, + const bool use_twist_input) { /// Save last linear and angular velocity: linear_ = v_bx; - angular_ = omega_bz; + angular_ = use_twist_input + ? last_angular_command_ + : convert_steering_angle_to_angular_velocity(v_bx, last_angular_command_); /// Integrate odometry: - integrate_fk(v_bx, omega_bz, dt); + integrate_fk(v_bx, last_angular_command_, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) @@ -231,9 +238,14 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome return std::isfinite(phi) ? phi : 0.0; } +double SteeringOdometry::convert_steering_angle_to_angular_velocity(double v_bx, double phi) +{ + return std::tan(phi) * v_bx / wheel_base_; +} + std::tuple, std::vector> SteeringOdometry::get_commands( const double v_bx, const double omega_bz, const bool open_loop, - const bool reduce_wheel_speed_until_steering_reached) + const bool reduce_wheel_speed_until_steering_reached, const bool use_twist_input) { // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; @@ -243,7 +255,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma { // TODO(anyone) this would be only possible if traction is on the steering axis phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; + Ws = abs(omega_bz) * wheel_base_ / wheel_radius_; } else { @@ -252,7 +264,8 @@ std::tuple, std::vector> SteeringOdometry::get_comma } #endif // steering angle - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + phi = + use_twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz) : omega_bz; if (open_loop) { phi_IK = phi; diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index 26e9330630..88282ff70e 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -5,11 +5,12 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false + use_twist_input: true traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] steering_joints_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 - front_wheel_track: 2.12321 - rear_wheel_track: 1.76868 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + steering_track_width: 2.12321 + traction_track_width: 1.76868 + steering_wheels_radius: 0.45 + traction_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml b/steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml new file mode 100644 index 0000000000..7515de850f --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml @@ -0,0 +1,16 @@ +test_steering_controllers_steering_input_library: + ros__parameters: + + reference_timeout: 0.1 + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_twist_input: false + traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + steering_track_width: 2.12321 + traction_track_width: 1.76868 + steering_wheels_radius: 0.45 + traction_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index f215526ee2..117fb58c07 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -105,7 +105,7 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - ControllerReferenceMsg msg; + ControllerTwistReferenceMsg msg; msg.header.stamp = controller_->get_node()->now(); msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; @@ -114,13 +114,13 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) msg.twist.angular.x = std::numeric_limits::quiet_NaN(); msg.twist.angular.y = std::numeric_limits::quiet_NaN(); msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); // age_of_last_command < ref_timeout_ auto age_of_last_command = - controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->input_ref_twist_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -129,8 +129,8 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); - ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + ASSERT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.angular.z)); // are the command_itfs updated? EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); @@ -147,24 +147,14 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) msg.twist.angular.x = std::numeric_limits::quiet_NaN(); msg.twist.angular.y = std::numeric_limits::quiet_NaN(); msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); - age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; - - // adjusting to achieve age_of_last_command > ref_timeout - msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; - msg.twist.linear.z = std::numeric_limits::quiet_NaN(); - msg.twist.angular.x = std::numeric_limits::quiet_NaN(); - msg.twist.angular.y = std::numeric_limits::quiet_NaN(); - msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set(msg); + age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->input_ref_twist_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -173,8 +163,8 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); - ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + ASSERT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.angular.z)); // Wheel velocities should reset to 0 EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); @@ -210,7 +200,7 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - ControllerReferenceMsg msg; + ControllerTwistReferenceMsg msg; msg.header.stamp = controller_->get_node()->now(); msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; @@ -219,14 +209,14 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) msg.twist.angular.x = std::numeric_limits::quiet_NaN(); msg.twist.angular.y = std::numeric_limits::quiet_NaN(); msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); auto age_of_last_command = - controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; // age_of_last_command < ref_timeout_ ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->input_ref_twist_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -235,8 +225,8 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); - ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + ASSERT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.angular.z)); // are the command_itfs updated? EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); @@ -253,13 +243,14 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) msg.twist.angular.x = std::numeric_limits::quiet_NaN(); msg.twist.angular.y = std::numeric_limits::quiet_NaN(); msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); - age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->input_ref_twist_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -268,8 +259,8 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); - ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + ASSERT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.angular.z)); // Wheel velocities should reset to 0 EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index e1b22ae0fa..ba4c326397 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -33,8 +33,10 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; -using ControllerReferenceMsg = +using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using ControllerSteeringReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg; // NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only @@ -88,7 +90,7 @@ class TestableSteeringControllersLibrary } /** - * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * @brief wait_for_command blocks until a new ControllerTwistReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. */ @@ -142,7 +144,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); - command_publisher_ = command_publisher_node_->create_publisher( + command_publisher_ = command_publisher_node_->create_publisher( "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); } @@ -287,7 +289,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); - ControllerReferenceMsg msg; + ControllerTwistReferenceMsg msg; msg.twist.linear.x = linear; msg.twist.angular.z = angular; @@ -328,7 +330,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Publisher::SharedPtr command_publisher_; }; #endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp new file mode 100644 index 0000000000..077d60f721 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -0,0 +1,223 @@ +// Copyright (c) 2023, FaSTTUBe - Formula Student Team TU Berlin +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_steering_controllers_library_steering_input.hpp" + +class SteeringControllersLibrarySteeringInputTest +: public SteeringControllersSteeringInputLibraryFixture< + TestableSteeringControllersSteeringInputLibrary> +{ +}; + +// checking if all interfaces, command, state and reference are exported as expected +TEST_F(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + traction_joints_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + traction_joints_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], + steering_joints_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], + steering_joints_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_LEFT_WHEEL], + controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + } + + // explicitly check the names of the two reference interfaces + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_POSITION); +} + +// Tests controller update_reference_from_subscribers and +// its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const float TEST_LINEAR_VELOCITY_X = static_cast(1.5); + const float TEST_STEERING_ANGLE = static_cast(0.575875); + + std::shared_ptr msg = + std::make_shared(); + + // First send a valid (non-timeout) message to establish command interface values + msg->header.stamp = controller_->get_node()->now(); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.set(*msg); + + // Process the valid message + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Now send the timeout message to test timeout behavior + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.set(*msg); + + const auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_steering_.get().header.stamp; + + // case 1 position_feedback = false + controller_->params_.position_feedback = false; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().linear_velocity)); + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().steering_angle)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); + + // case 2 position_feedback = true + controller_->params_.position_feedback = true; + + // First send a valid message for the position_feedback=true case + msg->header.stamp = controller_->get_node()->now(); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.set(*msg); + + // Process the valid message + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Now send the timeout message to test timeout behavior with position_feedback=true + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.set(*msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().linear_velocity)); + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().steering_angle)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp new file mode 100644 index 0000000000..9e535c1021 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp @@ -0,0 +1,344 @@ +// Copyright (c) 2025, FaSTTUBe - Formula Student Team TU Berlin +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_STEERING_INPUT_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_STEERING_INPUT_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; +using ControllerSteeringReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg; + +// NOTE: Testing steering_controllers_library for Steering vehicle configuration only + +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; +static constexpr double REAR_WHEEL_TRACK_ = 1.76868; +static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; +static constexpr double REAR_WHEELS_RADIUS_ = 0.45; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersSteeringInputLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces); + FRIEND_TEST( + SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerSteeringReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() + { + param_listener_ = std::make_shared(get_node()); + } + + controller_interface::CallbackReturn configure_odometry() + { + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_steering_controllers_steering_input_library/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController( + const std::string controller_name = "test_steering_controllers_steering_input_library") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + traction_joints_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + traction_joints_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + steering_joints_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + steering_joints_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + traction_joints_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + traction_joints_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + steering_joints_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + steering_joints_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_steering_controllers_steering_input_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands( + const float linear = static_cast(0.1), const float angular = static_cast(0.2)) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerSteeringReferenceMsg msg; + msg.linear_velocity = linear; + msg.steering_angle = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + std::vector traction_joints_names_ = { + "rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steering_joints_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0], + steering_joints_names_[1]}; + + std::vector traction_joints_preceding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector steering_joints_preceding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; + std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; + + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_STEERING_INPUT_HPP_ diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index a211ac8bf4..b33655779b 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -76,8 +76,22 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) odom.update_open_loop(1., -1., 1.); EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); + + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_LT(odom.get_y(), 0); // neg y, ie. right +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_ackermanndrive_angular_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 1., 1., false); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + double expected_angular = (1.0 / 2.0) * std::tan(1.0); + EXPECT_NEAR(odom.get_angular(), expected_angular, 1e-6); EXPECT_GT(odom.get_x(), 0); // pos x - EXPECT_LT(odom.get_y(), 0); // neg y ie. right + EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } TEST(TestSteeringOdometry, ackermann_IK_linear) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 43fdcd4421..30b3a8b8c8 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -47,7 +47,8 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop( + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input); } else { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 816a2181e8..2d399830bb 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -96,7 +96,7 @@ TEST_F(TricycleSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.get(); + auto msg = controller_->input_ref_twist_.get(); EXPECT_TRUE(std::isnan(msg.twist.linear.x)); EXPECT_TRUE(std::isnan(msg.twist.linear.y)); EXPECT_TRUE(std::isnan(msg.twist.linear.z)); @@ -158,7 +158,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) msg.header.stamp = controller_->get_node()->now(); msg.twist.linear.x = 0.1; msg.twist.angular.z = 0.2; - controller_->input_ref_.set(msg); + controller_->input_ref_twist_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -174,7 +174,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -211,7 +211,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) {