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..fd8c2bb445 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,10 @@ 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 `_] +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 `_] 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 83057652a5..a7578f6ef2 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; @@ -128,7 +134,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 be67eb9cdd..210c0a0063 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,24 @@ void reset_controller_reference_msg( msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } +void reset_controller_reference_msg( + const std::shared_ptr & 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(); +} + +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 @@ -113,7 +133,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two traction joints, but %zu were provided", + "Steering configuration requires exactly two traction joints, but %zu were provided", params_.traction_joints_names.size()); return controller_interface::CallbackReturn::ERROR; } @@ -147,7 +167,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two steering joints, but %zu were provided", + "Steering configuration requires exactly two steering joints, but %zu were provided", params_.steering_joints_names.size()); return controller_interface::CallbackReturn::ERROR; } @@ -188,7 +208,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two traction joints, but %zu state interface " + "Steering configuration requires exactly two traction joints, but %zu state interface " "names were provided", params_.traction_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; @@ -233,7 +253,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two steering joints, but %zu state interface " + "Steering configuration requires exactly two steering joints, but %zu state interface " "names were provided", params_.steering_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; @@ -253,12 +273,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 { @@ -340,7 +373,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 @@ -355,7 +388,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 { @@ -429,7 +490,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; @@ -442,8 +505,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; } @@ -468,42 +539,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()) + { + current_ref_steering_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - current_ref_steering_.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_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; + + 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 { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + 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_.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref_.twist.angular.z = 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_.try_set(current_ref_); + input_ref_steering_.try_set(current_ref_steering_); + } } } @@ -523,9 +646,20 @@ 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); + + // store (for open loop odometry) and set commands + last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; + last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; + auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, - params_.reduce_wheel_speed_until_steering_reached); + 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..8d223e18da 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); } @@ -185,11 +187,13 @@ bool SteeringOdometry::update_from_velocity( 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 omega_bz, const double dt, const bool use_twist_input) { /// Save last linear and angular velocity: linear_ = v_bx; - angular_ = omega_bz; + angular_ = + use_twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz); /// Integrate odometry: integrate_fk(v_bx, omega_bz, dt); @@ -231,9 +235,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 +252,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 +261,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 5aac580b33..e1e19e0e05 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..87d2454bcd --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -0,0 +1,223 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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..706494dadf --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp @@ -0,0 +1,345 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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::string preceeding_prefix_ = "pid_controller"; + + 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_) {