From d81c85b4c68ca6db18d8223639d250b21faa8fd3 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Tue, 11 Jun 2024 14:57:15 +0000 Subject: [PATCH] Included option to use Ackermann control via steering angle and linear velocity --- .../src/ackermann_steering_controller.cpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- .../steering_controllers_library.hpp | 6 +- .../steering_odometry.hpp | 12 +- .../src/steering_controllers_library.cpp | 140 +++++++++++++++--- .../src/steering_controllers_library.yaml | 7 + .../src/steering_odometry.cpp | 16 +- .../src/tricycle_steering_controller.cpp | 2 +- 8 files changed, 153 insertions(+), 34 deletions(-) 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/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 b560e2a782..d0f00dd51d 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 // store last velocity 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 2bea1d47f8..3ff3565f58 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -129,7 +129,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 @@ -184,10 +184,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); + const double v_bx, const double omega_bz, const bool open_loop, const bool twist_input=true); /** * \brief Reset poses, heading, and accumulators @@ -226,6 +227,13 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates steering angle from the desired twist + * \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 cf85ac3e41..82b81c28a7 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) { @@ -389,7 +450,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 +473,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 +539,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { // store 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..4bd41bd98d 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, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 97ef60a708..78fa696e42 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -175,11 +175,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); @@ -211,8 +211,13 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome return std::atan(omega_bz * wheelbase_ / v_bx); } +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_; @@ -232,6 +237,11 @@ std::tuple, std::vector> SteeringOdometry::get_comma #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 + if (!twist_input) + { + phi = omega_bz; + } 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 {