diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..af27b170e8 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -58,7 +58,7 @@ 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_command_, period.seconds(), params_.twist_input); } else { diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 6b64f901c3..702d03bfbf 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -7,6 +7,7 @@ test_ackermann_steering_controller: velocity_rolling_window_size: 10 position_feedback: false use_stamped_vel: true + twist_input: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index de45accc0a..182020b7b1 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -175,16 +175,62 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic_ackermann) +{ + 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); + controller_->get_node()->set_parameter( + rclcpp::Parameter("twist_input", false)); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->drive.speed = 0.1; + msg->drive.steering_angle = 0.2; + controller_->input_ackermann_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.1055, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.0944, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 0.188, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 0.215, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); @@ -235,6 +281,8 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) } } + + TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) { SetUpController(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index f9835a5e88..a29e0d33aa 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -37,6 +37,8 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using ControllerAckermannReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; // name constants for state interfaces using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL; @@ -68,6 +70,7 @@ class TestableAckermannSteeringController FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_ackermann); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chained); FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 95eaf1965c..6bd866e6ef 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -56,7 +56,7 @@ 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_command_, period.seconds(), params_.twist_input); } else { 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 29be21193e..d8f2472220 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 @@ -97,9 +97,10 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeBuffer> input_ackermann_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; @@ -134,7 +135,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // last velocity commands for open loop odometry double last_linear_velocity_ = 0.0; - double last_angular_velocity_ = 0.0; + double last_angular_command_ = 0.0; std::vector rear_wheels_state_names_; std::vector front_wheels_state_names_; @@ -144,6 +145,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); void reference_callback_unstamped(const std::shared_ptr msg); + void reference_callback_ackermann(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 12f4bd2f7b..1c0432a823 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -133,7 +133,7 @@ class SteeringOdometry * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call */ - 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 twist_input=true); /** * \brief Set odometry type @@ -188,10 +188,11 @@ class SteeringOdometry * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \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 twist_input If true, then omega_bz will be interpreted as steering angle * \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 double v_bx, const double omega_bz, const bool open_loop = true, const bool twist_input=true); /** * \brief Reset poses, heading, and accumulators @@ -230,6 +231,13 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates angular velocity from the desired steering angle + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param phi Steering angle of the robot around x_z-axis + */ + double convert_steering_angle_to_angular_velocity(const double v_bx, const 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 358cc7ae08..8c1a006f55 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -33,6 +33,9 @@ namespace using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using ControllerAckermannReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; + // called from RT control loop void reset_controller_reference_msg( const std::shared_ptr & msg, @@ -49,6 +52,18 @@ void reset_controller_reference_msg( } // namespace +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->drive.speed = std::numeric_limits::quiet_NaN(); + msg->drive.acceleration = std::numeric_limits::quiet_NaN(); + msg->drive.jerk = std::numeric_limits::quiet_NaN(); + msg->drive.steering_angle = std::numeric_limits::quiet_NaN(); + msg->drive.steering_angle_velocity = std::numeric_limits::quiet_NaN(); +} + namespace steering_controllers_library { SteeringControllersLibrary::SteeringControllersLibrary() @@ -114,12 +129,20 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) + if (params_.twist_input && params_.use_stamped_vel) { ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); } + else if (!params_.twist_input) + { + ref_subscriber_ackermann_ = + get_node()->create_subscription( + "~/reference_ackermann", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1)); + } else { RCLCPP_WARN( @@ -131,10 +154,20 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); } - std::shared_ptr msg = - std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + if (params_.twist_input) + { + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + } + else + { + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ackermann_ref_.writeFromNonRT(msg); + } try { @@ -244,6 +277,34 @@ void SteeringControllersLibrary::reference_callback( } } +void SteeringControllersLibrary::reference_callback_ackermann( + 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_ackermann_ref_.writeFromNonRT(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_unstamped( const std::shared_ptr msg) { @@ -344,10 +405,21 @@ SteeringControllersLibrary::state_interface_configuration() const } else { - for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + if(params_.use_front_wheel_feedback) + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + } + else + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) { state_interfaces_config.names.push_back( - front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } } for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) @@ -389,7 +461,12 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + if(params_.twist_input){ + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + } + else{ + reset_controller_reference_msg(*(input_ackermann_ref_.readFromRT()), get_node()); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -407,26 +484,52 @@ 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 = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; + if(params_.twist_input){ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; - // send 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.angular.z)) + // send 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.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; + } + } + else { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } } } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + else { + auto current_ref = *(input_ackermann_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle)) + { + reference_interfaces_[0] = current_ref->drive.speed; + reference_interfaces_[1] = current_ref->drive.steering_angle; + } + } + else + { + if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->drive.speed = std::numeric_limits::quiet_NaN(); + current_ref->drive.steering_angle = std::numeric_limits::quiet_NaN(); + } } } @@ -447,10 +550,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { // store (for open loop odometry) and set commands last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; - + last_angular_command_ = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); + odometry_.get_commands(last_linear_velocity_, last_angular_command_, params_.open_loop, params_.twist_input); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index b18cac5ae1..83a5373bdd 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -63,6 +63,13 @@ steering_controllers_library: read_only: false, } + twist_input: { + type: bool, + default_value: true, + description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.", + read_only: false, + } + velocity_rolling_window_size: { type: int, default_value: 10, @@ -113,6 +120,13 @@ steering_controllers_library: read_only: false, } + use_front_wheel_feedback: { + type: bool, + default_value: false, + description: "Choice which wheels to use for feedback, if front_wheel_feedback is false then rear wheels are used for feedback", + read_only: false, + } + use_stamped_vel: { type: bool, default_value: false, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index f0d4a9e926..eb961e2ada 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -181,11 +181,11 @@ 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 twist_input) { /// Save last linear and angular velocity: linear_ = v_bx; - angular_ = omega_bz; + angular_ = twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz); /// Integrate odometry: integrate_fk(v_bx, omega_bz, dt); @@ -214,8 +214,13 @@ 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 / wheelbase_; +} + std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz, const bool open_loop) + const double v_bx, const double omega_bz, const bool open_loop, const bool twist_input) { // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; @@ -233,8 +238,9 @@ std::tuple, std::vector> SteeringOdometry::get_comma Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle } #endif - // steering angle - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + // interprete twist input as steering angle if twist_input is false + phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz): omega_bz; + phi_IK = phi; if (open_loop) { phi_IK = phi; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..905bd01226 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -56,7 +56,7 @@ 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_command_, period.seconds(), params_.twist_input); } else {