From b107ac7bc94b3d88e2873ecbc57aa1cde3375eb5 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 22 Dec 2024 01:36:19 +0900 Subject: [PATCH] feat(raw_vehicle_cmd_converter): add vehicle adaptor (#8782) * feat(raw_vehicle_cmd_converter): add vehicle adaptor Signed-off-by: kosuke55 sub operation status Signed-off-by: kosuke55 * feat(raw_vehicle_cmd_converter): publish vehicle adaptor output Signed-off-by: kosuke55 * use control horizon Signed-off-by: kosuke55 * revert carla Signed-off-by: kosuke55 * update docs Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../CMakeLists.txt | 8 ++- .../README.md | 14 +++++ .../node.hpp | 17 +++++- .../vehicle_adaptor/vehicle_adaptor.hpp | 50 ++++++++++++++++ .../launch/raw_vehicle_converter.launch.xml | 6 ++ .../package.xml | 1 + .../raw_vehicle_cmd_converter.schema.json | 5 ++ .../src/node.cpp | 58 +++++++++++++------ .../src/vehicle_adaptor/vehicle_adaptor.cpp | 32 ++++++++++ 9 files changed, 169 insertions(+), 22 deletions(-) create mode 100644 vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp create mode 100644 vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt index 9e5b7439e1de2..909dd336bd282 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt @@ -13,11 +13,15 @@ ament_auto_add_library(actuation_map_converter SHARED src/vgr.cpp ) +ament_auto_add_library(vehicle_adaptor SHARED + src/vehicle_adaptor/vehicle_adaptor.cpp +) + ament_auto_add_library(autoware_raw_vehicle_cmd_converter_node_component SHARED src/node.cpp ) -target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component actuation_map_converter) +target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component actuation_map_converter vehicle_adaptor) rclcpp_components_register_node(autoware_raw_vehicle_cmd_converter_node_component PLUGIN "autoware::raw_vehicle_cmd_converter::RawVehicleCommandConverterNode" @@ -30,7 +34,7 @@ if(BUILD_TESTING) ) set(TEST_RAW_VEHICLE_CMD_CONVERTER_EXE test_autoware_raw_vehicle_cmd_converter) ament_add_ros_isolated_gtest(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter autoware_raw_vehicle_cmd_converter_node_component) + target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter vehicle_adaptor autoware_raw_vehicle_cmd_converter_node_component) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md index 1df083f5c5370..a4d72a684e7d3 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/README.md +++ b/vehicle/autoware_raw_vehicle_cmd_converter/README.md @@ -45,6 +45,12 @@ vgr_coef_c: 0.042 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. +### Vehicle Adaptor + +**Under development** +A feature that compensates for control commands according to the dynamic characteristics of the vehicle. +This feature works when `use_vehicle_adaptor: true` is set and requires `control_horizon` to be enabled, so you need to set `enable_control_cmd_horizon_pub: true` in the trajectory_follower node. + ## Input topics | Name | Type | Description | @@ -54,6 +60,14 @@ Also, when `convert_actuation_to_steering_status: true`, this node receives the | `~/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. | +Input topics when vehicle_adaptor is enabled + +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------- | ----------------------- | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped; | acceleration status | +| `~/input/operation_mode_state` | autoware_adapi_v1_msgs::msg::OperationModeState | operation mode status | +| `~/input/control_horizon` | autoware_control_msgs::msg::ControlHorizon | control horizon command | + ## Output topics | Name | Type | Description | 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 b5e13985c036e..8f8e543dc680e 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,10 +21,12 @@ #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/vehicle_adaptor/vehicle_adaptor.hpp" #include "autoware_raw_vehicle_cmd_converter/vgr.hpp" #include +#include #include #include #include @@ -46,7 +48,8 @@ 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; - +using autoware_adapi_v1_msgs::msg::OperationModeState; +using geometry_msgs::msg::AccelWithCovarianceStamped; class DebugValues { public: @@ -79,6 +82,7 @@ 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_; + rclcpp::Publisher::SharedPtr pub_compensated_control_cmd_; //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_actuation_status_; @@ -86,17 +90,27 @@ class RawVehicleCommandConverterNode : public rclcpp::Node // polling subscribers autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; + // polling subscribers for vehicle_adaptor + autoware::universe_utils::InterProcessPollingSubscriber sub_accel_{ + this, "~/input/accel"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; + // control_horizon is an experimental topic, but vehicle_adaptor uses it to improve performance, + autoware::universe_utils::InterProcessPollingSubscriber sub_control_horizon_{ + this, "~/input/control_horizon"}; rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; ActuationStatusStamped::ConstSharedPtr actuation_status_ptr_; + Odometry::ConstSharedPtr current_odometry_; Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; VGR vgr_; + VehicleAdaptor vehicle_adaptor_; // TODO(tanaka): consider accel/brake pid too PIDController steer_pid_; bool ff_map_initialized_; @@ -112,6 +126,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node 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}; + bool use_vehicle_adaptor_{false}; rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME}; // Whether to subscribe to actuation_status and calculate and publish steering_status diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp new file mode 100644 index 0000000000000..595cddd29817f --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp @@ -0,0 +1,50 @@ +// 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__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ +#define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace autoware::raw_vehicle_cmd_converter +{ + +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_control_msgs::msg::Control; +using autoware_control_msgs::msg::ControlHorizon; +using autoware_vehicle_msgs::msg::SteeringReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; + +class VehicleAdaptor +{ +public: + VehicleAdaptor() = default; + Control compensate( + const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry, + [[maybe_unused]] const AccelWithCovarianceStamped & accel, + [[maybe_unused]] const double steering, + [[maybe_unused]] const OperationModeState & operation_mode, + [[maybe_unused]] const ControlHorizon & control_horizon); + +private: +}; +} // namespace autoware::raw_vehicle_cmd_converter + +#endif // AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_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 441d924a0e6c3..dc96533c505cd 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 @@ -2,8 +2,11 @@ + + + @@ -13,8 +16,11 @@ + + + diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 468e039312520..ff00f09a7efd8 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -21,6 +21,7 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_control_msgs autoware_interpolation autoware_vehicle_msgs 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 30642663a39f5..84bc78e1e7ace 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 @@ -180,6 +180,11 @@ "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." + }, + "use_vehicle_adaptor": { + "type": "boolean", + "default": "false", + "description": "flag to enable feature that compensates control commands according to vehicle dynamics." } }, "required": [ diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index e4cdbe60fd4cd..7624705b15b37 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -40,6 +40,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( // for steering steer controller use_steer_ff_ = declare_parameter("use_steer_ff"); use_steer_fb_ = declare_parameter("use_steer_fb"); + use_vehicle_adaptor_ = declare_parameter("use_vehicle_adaptor", false); + if (convert_accel_cmd_) { if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) { throw std::invalid_argument("Accel map is invalid."); @@ -116,15 +118,21 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); + if (use_vehicle_adaptor_) { + pub_compensated_control_cmd_ = create_publisher( + "/vehicle/raw_vehicle_cmd_converter/debug/compensated_control_cmd", 1); + } + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() { - if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) { + /* check if all necessary data is available */ + if (!current_odometry_ || !control_cmd_ptr_ || !current_steer_ptr_) { RCLCPP_WARN_EXPRESSION( get_logger(), is_debugging_, "some pointers are null: %s, %s, %s", - !current_twist_ptr_ ? "twist" : "", !control_cmd_ptr_ ? "cmd" : "", + !current_odometry_ ? "odometry" : "", !control_cmd_ptr_ ? "cmd" : "", !current_steer_ptr_ ? "steer" : ""); return; } @@ -136,14 +144,34 @@ void RawVehicleCommandConverterNode::publishActuationCmd() } } + /* compensate control command if vehicle adaptor is enabled */ + Control control_cmd = *control_cmd_ptr_; + if (use_vehicle_adaptor_) { + const auto current_accel = sub_accel_.takeData(); + const auto current_operation_mode = sub_operation_mode_.takeData(); + const auto control_horizon = sub_control_horizon_.takeData(); + if (!current_accel || !current_operation_mode || !control_horizon) { + RCLCPP_WARN_EXPRESSION( + get_logger(), is_debugging_, "some pointers are null: %s, %s %s", + !current_accel ? "accel" : "", !current_operation_mode ? "operation_mode" : "", + !control_horizon ? "control_horizon" : ""); + return; + } + control_cmd = vehicle_adaptor_.compensate( + *control_cmd_ptr_, *current_odometry_, *current_accel, *current_steer_ptr_, + *current_operation_mode, *control_horizon); + pub_compensated_control_cmd_->publish(control_cmd); + } + + /* calculate actuation command */ double desired_accel_cmd = 0.0; double desired_brake_cmd = 0.0; double desired_steer_cmd = 0.0; ActuationCommandStamped actuation_cmd; - const double acc = control_cmd_ptr_->longitudinal.acceleration; - const double vel = current_twist_ptr_->twist.linear.x; - const double steer = control_cmd_ptr_->lateral.steering_tire_angle; - const double steer_rate = control_cmd_ptr_->lateral.steering_tire_rotation_rate; + const double acc = control_cmd.longitudinal.acceleration; + const double vel = current_odometry_->twist.twist.linear.x; + const double steer = control_cmd.lateral.steering_tire_angle; + const double steer_rate = control_cmd.lateral.steering_tire_rotation_rate; bool accel_cmd_is_zero = true; if (convert_accel_cmd_) { desired_accel_cmd = calculateAccelMap(vel, acc, accel_cmd_is_zero); @@ -173,7 +201,7 @@ void RawVehicleCommandConverterNode::publishActuationCmd() desired_steer_cmd = calculateSteerFromMap(vel, steer, steer_rate); } actuation_cmd.header.frame_id = "base_link"; - actuation_cmd.header.stamp = control_cmd_ptr_->stamp; + actuation_cmd.header.stamp = control_cmd.stamp; actuation_cmd.actuation.accel_cmd = desired_accel_cmd; actuation_cmd.actuation.brake_cmd = desired_brake_cmd; actuation_cmd.actuation.steer_cmd = desired_steer_cmd; @@ -252,12 +280,7 @@ double RawVehicleCommandConverterNode::calculateBrakeMap( void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { - const auto odometry_msg = sub_odometry_.takeData(); - if (odometry_msg) { - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = odometry_msg->header; - current_twist_ptr_->twist = odometry_msg->twist.twist; - } + current_odometry_ = sub_odometry_.takeData(); control_cmd_ptr_ = msg; publishActuationCmd(); } @@ -277,14 +300,11 @@ void RawVehicleCommandConverterNode::onActuationStatus( } // calculate steering status from actuation status - const auto odometry_msg = sub_odometry_.takeData(); - if (odometry_msg) { + current_odometry_ = sub_odometry_.takeData(); + if (current_odometry_) { 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)); + current_odometry_->twist.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); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp new file mode 100644 index 0000000000000..7bc9076ae944b --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp @@ -0,0 +1,32 @@ +// 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/vehicle_adaptor/vehicle_adaptor.hpp" + +#include + +namespace autoware::raw_vehicle_cmd_converter +{ +Control VehicleAdaptor::compensate( + const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry, + [[maybe_unused]] const AccelWithCovarianceStamped & accel, [[maybe_unused]] const double steering, + [[maybe_unused]] const OperationModeState & operation_mode, + [[maybe_unused]] const ControlHorizon & control_horizon) +{ + // TODO(someone): implement the control command compensation + Control output_control_cmd = input_control_cmd; + std::cerr << "vehicle adaptor: compensate control command" << std::endl; + return output_control_cmd; +} +} // namespace autoware::raw_vehicle_cmd_converter