From d95cfaecd0e46cabe121a3bd3edc3f56fab2ef73 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Aug 2024 13:59:52 +0900 Subject: [PATCH] feat(raw_vehicle_cmd_converter): add steer command conversion with VGR (#8504) * feat(raw_vehicle_cmd_converter): add steer command conversion with VGR Signed-off-by: kosuke55 * make class and add test Signed-off-by: kosuke55 * remove member vgr_coef from node Signed-off-by: kosuke55 * update readme Signed-off-by: kosuke55 * add svg Signed-off-by: kosuke55 * add plot scripts Signed-off-by: kosuke55 * Update vehicle/autoware_raw_vehicle_cmd_converter/README.md Co-authored-by: Takamasa Horibe * not always subscribe actuation_status Signed-off-by: kosuke55 * add comment for using normal sub for steering status Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Takamasa Horibe --- .../CMakeLists.txt | 1 + .../README.md | 39 +- .../raw_vehicle_cmd_converter.param.yaml | 5 + .../figure/vgr.svg | 3541 +++++++++++++++++ .../node.hpp | 26 +- .../vgr.hpp | 39 + .../launch/raw_vehicle_converter.launch.xml | 4 + .../raw_vehicle_cmd_converter.schema.json | 33 +- .../scripts/plot_variable_gear_ratio.py | 107 + .../src/node.cpp | 147 +- .../src/vgr.cpp | 41 + ...est_autoware_raw_vehicle_cmd_converter.cpp | 48 + 12 files changed, 3982 insertions(+), 49 deletions(-) create mode 100644 vehicle/autoware_raw_vehicle_cmd_converter/figure/vgr.svg create mode 100644 vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vgr.hpp create mode 100755 vehicle/autoware_raw_vehicle_cmd_converter/scripts/plot_variable_gear_ratio.py create mode 100644 vehicle/autoware_raw_vehicle_cmd_converter/src/vgr.cpp diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt index fc54b302512fa..9e5b7439e1de2 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(actuation_map_converter SHARED src/steer_map.cpp src/csv_loader.cpp src/pid.cpp + src/vgr.cpp ) ament_auto_add_library(autoware_raw_vehicle_cmd_converter_node_component SHARED diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md index 858db3cbaa768..1df083f5c5370 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/README.md +++ b/vehicle/autoware_raw_vehicle_cmd_converter/README.md @@ -24,19 +24,42 @@ Once the acceleration map is crafted, it should be loaded when the RawVehicleCmd For ease of calibration and adjustments to the lookup table, an auto-calibration tool is available. More information and instructions for this tool can be found [here](https://github.com/autowarefoundation/autoware.universe/blob/main/vehicle/autoware_accel_brake_map_calibrator/README.md). +### Variable Gear Ratio (VGR) + +This is a gear ratio for converting tire angle to steering angle. Generally, to improve operability, the gear ratio becomes dynamically larger as the speed increases or the steering angle becomes smaller. For a certain vehicle, data was acquired and the gear ratio was approximated by the following formula. + +$$ +a + b \times v^2 - c \times \lvert \delta \rvert +$$ + +For that vehicle, the coefficients were as follows. + +```yaml +vgr_coef_a: 15.713 +vgr_coef_b: 0.053 +vgr_coef_c: 0.042 +``` + +![vgr](./figure/vgr.svg) + +When `convert_steer_cmd_method: "vgr"` is selected, the node receives the control command from the controller as the desired tire angle and calculates the desired steering angle to output. +Also, when `convert_actuation_to_steering_status: true`, this node receives the `actuation_status` topic and calculates the steer tire angle from the `steer_wheel_angle` and publishes it. + ## Input topics -| Name | Type | Description | -| --------------------- | ------------------------------------------ | ------------------------------------------------------------------------------------------------------------------ | -| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | -| `~/input/steering"` | autoware_vehicle_msgs::msg::SteeringReport | current status of steering used for steering feed back control | -| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | +| Name | Type | Description | +| -------------------------- | ------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | +| `~/input/steering"` | autoware_vehicle_msgs::msg::SteeringReport | subscribe only when `convert_actuation_to_steering_status: false`. current status of steering used for steering feed back control | +| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | +| `~/input/actuation_status` | tier4_vehicle_msgs::msg::ActuationStatus | actuation status is assumed to receive the same type of status as sent to the vehicle side. For example, if throttle/brake pedal/steer_wheel_angle is sent, the same type of status is received. In the case of steer_wheel_angle, it is used to calculate steer_tire_angle and VGR in this node. | ## Output topics -| Name | Type | Description | -| ------------------------ | ------------------------------------------------ | ------------------------------------------------------- | -| `~/output/actuation_cmd` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation command for vehicle to apply mechanical input | +| Name | Type | Description | +| -------------------------- | ------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------ | +| `~/output/actuation_cmd` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation command for vehicle to apply mechanical input | +| `~/output/steering_status` | autoware_vehicle_msgs::msg::SteeringReport | publish only when `convert_actuation_to_steering_status: true`. steer tire angle is calculated from steer wheel angle and published. | ## Parameters diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index b53b0d7622198..e7e503d6d7eee 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -26,3 +26,8 @@ max_d: 0.0 min_d: 0.0 invalid_integration_decay: 0.97 + convert_steer_cmd_method: "vgr" # "vgr" or "steer_map" + vgr_coef_a: 15.713 + vgr_coef_b: 0.053 + vgr_coef_c: 0.042 + convert_actuation_to_steering_status: true diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/figure/vgr.svg b/vehicle/autoware_raw_vehicle_cmd_converter/figure/vgr.svg new file mode 100644 index 0000000000000..51bf3e56d2e6a --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/figure/vgr.svg @@ -0,0 +1,3541 @@ + + + + + + + + 2024-08-19T15:08:36.202767 + image/svg+xml + + + Matplotlib v3.5.1, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index 181b7926337dc..b5e13985c036e 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -21,6 +21,7 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "autoware_raw_vehicle_cmd_converter/pid.hpp" #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" +#include "autoware_raw_vehicle_cmd_converter/vgr.hpp" #include @@ -30,6 +31,7 @@ #include #include #include +#include #include #include @@ -40,6 +42,7 @@ namespace autoware::raw_vehicle_cmd_converter using Control = autoware_control_msgs::msg::Control; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_vehicle_msgs::msg::ActuationCommandStamped; +using tier4_vehicle_msgs::msg::ActuationStatusStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Odometry = nav_msgs::msg::Odometry; using Steering = autoware_vehicle_msgs::msg::SteeringReport; @@ -75,22 +78,25 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief topic publisher for low level vehicle command rclcpp::Publisher::SharedPtr pub_actuation_cmd_; + rclcpp::Publisher::SharedPtr pub_steering_status_; //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_actuation_status_; + rclcpp::Subscription::SharedPtr sub_steering_; // polling subscribers autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_steering_{ - this, "~/input/steering"}; rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; + ActuationStatusStamped::ConstSharedPtr actuation_status_ptr_; Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; + VGR vgr_; // TODO(tanaka): consider accel/brake pid too PIDController steer_pid_; bool ff_map_initialized_; @@ -102,16 +108,24 @@ class RawVehicleCommandConverterNode : public rclcpp::Node bool use_steer_ff_; bool use_steer_fb_; bool is_debugging_; - bool convert_accel_cmd_; //!< @brief use accel or not - bool convert_brake_cmd_; //!< @brief use brake or not - bool convert_steer_cmd_; //!< @brief use steer or not + bool convert_accel_cmd_; //!< @brief use accel or not + bool convert_brake_cmd_; //!< @brief use brake or not + std::optional convert_steer_cmd_method_{std::nullopt}; //!< @brief method to convert + bool need_to_subscribe_actuation_status_{false}; rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME}; + // Whether to subscribe to actuation_status and calculate and publish steering_status + // For example, receive the steering wheel angle and calculate the steering wheel angle based on + // the gear ratio. If false, the vehicle interface must publish steering_status. + bool convert_actuation_to_steering_status_{false}; // !< @brief use actuation_status or not + double calculateAccelMap( const double current_velocity, const double desired_acc, bool & accel_cmd_is_zero); double calculateBrakeMap(const double current_velocity, const double desired_acc); - double calculateSteer(const double vel, const double steering, const double steer_rate); + double calculateSteerFromMap(const double vel, const double steering, const double steer_rate); void onControlCmd(const Control::ConstSharedPtr msg); + void onSteering(const Steering::ConstSharedPtr msg); + void onActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); void publishActuationCmd(); // for debugging rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vgr.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vgr.hpp new file mode 100644 index 0000000000000..3b795c4d51036 --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vgr.hpp @@ -0,0 +1,39 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// 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 AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VGR_HPP_ +#define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VGR_HPP_ + +namespace autoware::raw_vehicle_cmd_converter +{ +class VGR +{ +public: + VGR() = default; + VGR(const double vgr_coef_a, const double vgr_coef_b, const double vgr_coef_c) + : vgr_coef_a_(vgr_coef_a), vgr_coef_b_(vgr_coef_b), vgr_coef_c_(vgr_coef_c) + { + } + void setCoefficients(double a, double b, double c); + double calculateVariableGearRatio(double vel, double steer_wheel) const; + double calculateSteeringTireState(double vel, double steer_wheel) const; + +private: + double vgr_coef_a_{0.0}; + double vgr_coef_b_{0.0}; + double vgr_coef_c_{0.0}; +}; +} // namespace autoware::raw_vehicle_cmd_converter + +#endif // AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VGR_HPP_ diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index 735d51e80d5d4..441d924a0e6c3 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -3,7 +3,9 @@ + + @@ -12,6 +14,8 @@ + + diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index 1903f8252d656..30642663a39f5 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -154,6 +154,32 @@ "min_d", "invalid_integration_decay" ] + }, + "convert_steer_cmd_method": { + "type": "string", + "description": "method for converting steer command", + "default": "vgr", + "enum": ["vgr", "steer_map"] + }, + "vgr_coef_a": { + "type": "number", + "default": "15.713", + "description": "coefficient a for variable gear ratio" + }, + "vgr_coef_b": { + "type": "number", + "default": "0.053", + "description": "coefficient b for variable gear ratio" + }, + "vgr_coef_c": { + "type": "number", + "default": "0.042", + "description": "coefficient c for variable gear ratio" + }, + "convert_actuation_to_steering_status": { + "type": "boolean", + "default": "true", + "description": "convert actuation to steering status or not. Whether to subscribe to actuation_status and calculate and publish steering_status For example, receive the steering wheel angle and calculate the steering wheel angle based on the gear ratio. If false, the vehicle interface must publish steering_status." } }, "required": [ @@ -169,7 +195,12 @@ "max_throttle", "max_brake", "max_steer", - "steer_pid" + "steer_pid", + "convert_steer_cmd_method", + "vgr_coef_a", + "vgr_coef_b", + "vgr_coef_c", + "convert_actuation_to_steering_status" ] } }, diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/scripts/plot_variable_gear_ratio.py b/vehicle/autoware_raw_vehicle_cmd_converter/scripts/plot_variable_gear_ratio.py new file mode 100755 index 0000000000000..f7f8cd6cb3395 --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/scripts/plot_variable_gear_ratio.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# 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. + +import argparse + +import matplotlib.pyplot as plt +import numpy as np + + +def calculate_variable_gear_ratio(vel, steer_wheel, a, b, c): + return max(1e-5, a + b * vel * vel - c * abs(steer_wheel)) + + +def plot_vgr_vs_steer_wheel(ax, velocity, a, b, c): + steer_wheel_values = np.linspace(-10.0, 10.0, 100) + vgr_values = [ + calculate_variable_gear_ratio(velocity, steer_wheel, a, b, c) + for steer_wheel in steer_wheel_values + ] + + ax.plot( + steer_wheel_values, + vgr_values, + linewidth=2, + ) + ax.set_xlabel("Steer Wheel [rad]", fontsize=14) + ax.set_ylabel("Variable Gear Ratio", fontsize=14) + ax.set_title( + f"VGR vs Steer Wheel\n(Velocity = {velocity} m/s, a = {a}, b = {b}, c = {c})", + fontsize=16, + fontweight="bold", + ) + ax.grid(True, linestyle="--", alpha=0.7) + ax.tick_params(axis="both", which="major", labelsize=12) + + +def plot_vgr_vs_velocity(ax, steer_wheel, a, b, c): + velocity_values = np.linspace(0, 16.66, 100) + vgr_values = [ + calculate_variable_gear_ratio(velocity, steer_wheel, a, b, c) + for velocity in velocity_values + ] + + ax.plot(velocity_values, vgr_values, linewidth=2) + ax.set_xlabel("Velocity [m/s]", fontsize=14) + ax.set_ylabel("Variable Gear Ratio", fontsize=14) + ax.set_title( + f"VGR vs Velocity\n(Steer Wheel = {steer_wheel} rad, a = {a}, b = {b}, c = {c})", + fontsize=16, + fontweight="bold", + ) + ax.grid(True, linestyle="--", alpha=0.7) + ax.tick_params(axis="both", which="major", labelsize=12) + + +def main(): + parser = argparse.ArgumentParser( + description="Plot Variable Gear Ratio (VGR) based on velocity and steer wheel angle." + ) + parser.add_argument("--vgr_coef_a", type=float, default=15.713, help="Coefficient a for VGR") + parser.add_argument("--vgr_coef_b", type=float, default=0.053, help="Coefficient b for VGR") + parser.add_argument("--vgr_coef_c", type=float, default=0.042, help="Coefficient c for VGR") + parser.add_argument( + "--velocity", + type=float, + default=8.33, + help="Fixed velocity value for plotting VGR vs Steer Wheel", + ) + parser.add_argument( + "--steer_wheel", + type=float, + default=0.0, + help="Fixed steer wheel value for plotting VGR vs Velocity", + ) + + args = parser.parse_args() + + plt.style.use("seaborn-whitegrid") + + fig, axs = plt.subplots(1, 2, figsize=(16, 8)) + + plot_vgr_vs_steer_wheel( + axs[0], args.velocity, args.vgr_coef_a, args.vgr_coef_b, args.vgr_coef_c + ) + plot_vgr_vs_velocity( + axs[1], args.steer_wheel, args.vgr_coef_a, args.vgr_coef_b, args.vgr_coef_c + ) + + plt.tight_layout() + plt.show() + + +if __name__ == "__main__": + main() diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index c4d7ea11bbcf3..6fb8f4dff44f6 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -29,10 +29,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( /* parameters for accel/brake map */ const auto csv_path_accel_map = declare_parameter("csv_path_accel_map"); const auto csv_path_brake_map = declare_parameter("csv_path_brake_map"); - const auto csv_path_steer_map = declare_parameter("csv_path_steer_map"); convert_accel_cmd_ = declare_parameter("convert_accel_cmd"); convert_brake_cmd_ = declare_parameter("convert_brake_cmd"); - convert_steer_cmd_ = declare_parameter("convert_steer_cmd"); max_accel_cmd_ = declare_parameter("max_throttle"); max_brake_cmd_ = declare_parameter("max_brake"); max_steer_cmd_ = declare_parameter("max_steer"); @@ -51,33 +49,68 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( throw std::invalid_argument("Brake map is invalid."); } } - if (convert_steer_cmd_) { - if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map, true)) { - throw std::invalid_argument("Steer map is invalid."); + if (declare_parameter("convert_steer_cmd")) { + convert_steer_cmd_method_ = declare_parameter("convert_steer_cmd_method", "vgr"); + if (convert_steer_cmd_method_.value() == "vgr") { + const double a = declare_parameter("vgr_coef_a"); + const double b = declare_parameter("vgr_coef_b"); + const double c = declare_parameter("vgr_coef_c"); + vgr_.setCoefficients(a, b, c); + } else if (convert_steer_cmd_method_.value() == "steer_map") { + const auto csv_path_steer_map = declare_parameter("csv_path_steer_map"); + if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map, true)) { + throw std::invalid_argument("Steer map is invalid."); + } + const auto kp_steer{declare_parameter("steer_pid.kp")}; + const auto ki_steer{declare_parameter("steer_pid.ki")}; + const auto kd_steer{declare_parameter("steer_pid.kd")}; + const auto max_ret_steer{declare_parameter("steer_pid.max")}; + const auto min_ret_steer{declare_parameter("steer_pid.min")}; + const auto max_ret_p_steer{declare_parameter("steer_pid.max_p")}; + const auto min_ret_p_steer{declare_parameter("steer_pid.min_p")}; + const auto max_ret_i_steer{declare_parameter("steer_pid.max_i")}; + const auto min_ret_i_steer{declare_parameter("steer_pid.min_i")}; + const auto max_ret_d_steer{declare_parameter("steer_pid.max_d")}; + const auto min_ret_d_steer{declare_parameter("steer_pid.min_d")}; + const auto invalid_integration_decay{ + declare_parameter("steer_pid.invalid_integration_decay")}; + steer_pid_.setDecay(invalid_integration_decay); + steer_pid_.setGains(kp_steer, ki_steer, kd_steer); + steer_pid_.setLimits( + max_ret_steer, min_ret_steer, max_ret_p_steer, min_ret_p_steer, max_ret_i_steer, + min_ret_i_steer, max_ret_d_steer, min_ret_d_steer); + steer_pid_.setInitialized(); + } else { + throw std::invalid_argument("Invalid steer conversion method."); } - const auto kp_steer{declare_parameter("steer_pid.kp")}; - const auto ki_steer{declare_parameter("steer_pid.ki")}; - const auto kd_steer{declare_parameter("steer_pid.kd")}; - const auto max_ret_steer{declare_parameter("steer_pid.max")}; - const auto min_ret_steer{declare_parameter("steer_pid.min")}; - const auto max_ret_p_steer{declare_parameter("steer_pid.max_p")}; - const auto min_ret_p_steer{declare_parameter("steer_pid.min_p")}; - const auto max_ret_i_steer{declare_parameter("steer_pid.max_i")}; - const auto min_ret_i_steer{declare_parameter("steer_pid.min_i")}; - const auto max_ret_d_steer{declare_parameter("steer_pid.max_d")}; - const auto min_ret_d_steer{declare_parameter("steer_pid.min_d")}; - const auto invalid_integration_decay{ - declare_parameter("steer_pid.invalid_integration_decay")}; - steer_pid_.setDecay(invalid_integration_decay); - steer_pid_.setGains(kp_steer, ki_steer, kd_steer); - steer_pid_.setLimits( - max_ret_steer, min_ret_steer, max_ret_p_steer, min_ret_p_steer, max_ret_i_steer, - min_ret_i_steer, max_ret_d_steer, min_ret_d_steer); - steer_pid_.setInitialized(); } - pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); + + // NOTE: The steering status can be published from the vehicle side or converted in this node. + convert_actuation_to_steering_status_ = + declare_parameter("convert_actuation_to_steering_status"); + if (convert_actuation_to_steering_status_) { + pub_steering_status_ = create_publisher("~/output/steering_status", 1); + } else { + // NOTE: Polling subscriber requires specifying the topic name at declaration, + // so use a normal callback subscriber. + sub_steering_ = create_subscription( + "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1)); + } + + // NOTE: some vehicles do not publish actuation status. To handle this, subscribe only when the + // option is specified. + need_to_subscribe_actuation_status_ = + convert_actuation_to_steering_status_ || convert_steer_cmd_method_.value() == "vgr"; + if (need_to_subscribe_actuation_status_) { + sub_actuation_status_ = create_subscription( + "~/input/actuation_status", 1, + std::bind(&RawVehicleCommandConverterNode::onActuationStatus, this, _1)); + } + sub_control_cmd_ = create_subscription( "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1)); + + pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); @@ -86,13 +119,21 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( void RawVehicleCommandConverterNode::publishActuationCmd() { - if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) { + if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_ || !actuation_status_ptr_) { RCLCPP_WARN_EXPRESSION( get_logger(), is_debugging_, "some pointers are null: %s, %s, %s", !current_twist_ptr_ ? "twist" : "", !control_cmd_ptr_ ? "cmd" : "", !current_steer_ptr_ ? "steer" : ""); return; } + + if (need_to_subscribe_actuation_status_) { + if (!actuation_status_ptr_) { + RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "actuation status is null"); + return; + } + } + double desired_accel_cmd = 0.0; double desired_brake_cmd = 0.0; double desired_steer_cmd = 0.0; @@ -116,11 +157,18 @@ void RawVehicleCommandConverterNode::publishActuationCmd() // if conversion is disabled use negative acceleration as brake cmd desired_brake_cmd = -acc; } - if (convert_steer_cmd_) { - desired_steer_cmd = calculateSteer(vel, steer, steer_rate); - } else { + if (!convert_steer_cmd_method_.has_value()) { // if conversion is disabled use steering angle as steer cmd desired_steer_cmd = steer; + } else if (convert_steer_cmd_method_.value() == "vgr") { + // NOTE: When using variable gear ratio, + // the actuation cmd is the steering wheel angle, + // and the actuation_status is also the steering wheel angle. + const double current_steer_wheel = actuation_status_ptr_->status.steer_status; + const double adaptive_gear_ratio = vgr_.calculateVariableGearRatio(vel, current_steer_wheel); + desired_steer_cmd = steer * adaptive_gear_ratio; + } else if (convert_steer_cmd_method_.value() == "steer_map") { + desired_steer_cmd = calculateSteerFromMap(vel, steer, steer_rate); } actuation_cmd.header.frame_id = "base_link"; actuation_cmd.header.stamp = control_cmd_ptr_->stamp; @@ -130,7 +178,7 @@ void RawVehicleCommandConverterNode::publishActuationCmd() pub_actuation_cmd_->publish(actuation_cmd); } -double RawVehicleCommandConverterNode::calculateSteer( +double RawVehicleCommandConverterNode::calculateSteerFromMap( const double vel, const double steering, const double steer_rate) { double steering_output = 0; @@ -203,10 +251,6 @@ double RawVehicleCommandConverterNode::calculateBrakeMap( void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { const auto odometry_msg = sub_odometry_.takeData(); - const auto steering_msg = sub_steering_.takeData(); - if (steering_msg) { - current_steer_ptr_ = std::make_unique(steering_msg->steering_tire_angle); - } if (odometry_msg) { current_twist_ptr_ = std::make_unique(); current_twist_ptr_->header = odometry_msg->header; @@ -215,6 +259,41 @@ void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr control_cmd_ptr_ = msg; publishActuationCmd(); } + +void RawVehicleCommandConverterNode::onSteering(const Steering::ConstSharedPtr msg) +{ + current_steer_ptr_ = std::make_unique(msg->steering_tire_angle); +} + +void RawVehicleCommandConverterNode::onActuationStatus( + const ActuationStatusStamped::ConstSharedPtr msg) +{ + actuation_status_ptr_ = msg; + + if (!convert_actuation_to_steering_status_) { + return; + } + + // calculate steering status from actuation status + const auto odometry_msg = sub_odometry_.takeData(); + if (odometry_msg) { + if (convert_steer_cmd_method_.value() == "vgr") { + current_twist_ptr_ = std::make_unique(); + current_twist_ptr_->header = odometry_msg->header; + current_twist_ptr_->twist = odometry_msg->twist.twist; + current_steer_ptr_ = std::make_unique(vgr_.calculateSteeringTireState( + current_twist_ptr_->twist.linear.x, actuation_status_ptr_->status.steer_status)); + Steering steering_msg{}; + steering_msg.steering_tire_angle = *current_steer_ptr_; + pub_steering_status_->publish(steering_msg); + } else if (convert_steer_cmd_method_.value() == "steer_map") { + throw std::domain_error( + "Steer map conversion is not supported for actuation status. Please " + "use vgr conversion method or set convert_actuation_to_steering to " + "false."); + } + } +} } // namespace autoware::raw_vehicle_cmd_converter #include diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/vgr.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/vgr.cpp new file mode 100644 index 0000000000000..13463d1678ac8 --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/vgr.cpp @@ -0,0 +1,41 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// 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 "autoware_raw_vehicle_cmd_converter/vgr.hpp" + +#include +#include + +namespace autoware::raw_vehicle_cmd_converter +{ + +void VGR::setCoefficients(const double a, const double b, const double c) +{ + vgr_coef_a_ = a; + vgr_coef_b_ = b; + vgr_coef_c_ = c; +} + +double VGR::calculateVariableGearRatio(const double vel, const double steer_wheel) const +{ + return std::max( + 1e-5, vgr_coef_a_ + vgr_coef_b_ * vel * vel - vgr_coef_c_ * std::fabs(steer_wheel)); +} + +double VGR::calculateSteeringTireState(const double vel, const double steer_wheel) const +{ + const double adaptive_gear_ratio = calculateVariableGearRatio(vel, steer_wheel); + return steer_wheel / adaptive_gear_ratio; +} +} // namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp index 746f872789b07..59ab3a880fabd 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp @@ -17,6 +17,7 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "autoware_raw_vehicle_cmd_converter/pid.hpp" #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" +#include "autoware_raw_vehicle_cmd_converter/vgr.hpp" #include "gtest/gtest.h" #include @@ -52,6 +53,7 @@ using autoware::raw_vehicle_cmd_converter::AccelMap; using autoware::raw_vehicle_cmd_converter::BrakeMap; using autoware::raw_vehicle_cmd_converter::PIDController; using autoware::raw_vehicle_cmd_converter::SteerMap; +using autoware::raw_vehicle_cmd_converter::VGR; double epsilon = 1e-4; // may throw PackageNotFoundError exception for invalid package const auto map_path = @@ -297,3 +299,49 @@ TEST(PIDTests, calculateFB) EXPECT_NEAR(pid_contributions.at(1), 0.21825, epsilon); EXPECT_NEAR(pid_contributions.at(2), -0.15, epsilon); } + +TEST(VGRTests, roundTrip) +{ + VGR vgr; + vgr.setCoefficients(15.713, 0.053, 0.042); + double vel = 5.0; + double steer_wheel = 0.1; + double gear_ratio = vgr.calculateVariableGearRatio(vel, steer_wheel); + double steer = vgr.calculateSteeringTireState(vel, steer_wheel); + double steer_wheel2 = steer * gear_ratio; + EXPECT_NEAR(steer_wheel, steer_wheel2, epsilon); +} + +TEST(VGRTests, boundaryValues) +{ + VGR vgr; + vgr.setCoefficients(15.713, 0.053, 0.042); + + const double vel = 0.0; + const double steer_wheel = 0.0; + const double gear_ratio = vgr.calculateVariableGearRatio(vel, steer_wheel); + EXPECT_NEAR(gear_ratio, 15.713, epsilon); + + const double steer_wheel_small = 1e-5; + const double steer = vgr.calculateSteeringTireState(vel, steer_wheel_small); + const double steer_wheel2 = steer * gear_ratio; + EXPECT_NEAR(steer_wheel, steer_wheel2, epsilon); +} + +TEST(VGRTests, zeroCoefficients) +{ + VGR vgr; + vgr.setCoefficients(0.0, 0.0, 0.0); + + const double vel = 10.0; + const double steer_wheel = 0.5; + + // Gear ratio should return the minimum value since all coefficients are zero + const double gear_ratio = vgr.calculateVariableGearRatio(vel, steer_wheel); + EXPECT_EQ(gear_ratio, 1e-5); + + // Steering tire state calculation is also performed with the minimum gear ratio + const double steer = vgr.calculateSteeringTireState(vel, steer_wheel); + const double steer_wheel2 = steer * gear_ratio; + EXPECT_NEAR(steer_wheel, steer_wheel2, epsilon); +}