From aabceea3e02e9cb5e14859302006fe1e663cc712 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 16 Aug 2024 18:30:51 +0900 Subject: [PATCH 1/4] feat(pacmod_interface): Additional option to receive wheel angle directly Signed-off-by: kosuke55 --- pacmod_interface/config/pacmod.param.yaml | 2 ++ .../pacmod_interface/pacmod_interface.hpp | 7 +++++ .../src/pacmod_interface/pacmod_interface.cpp | 27 ++++++++++++++----- 3 files changed, 29 insertions(+), 7 deletions(-) diff --git a/pacmod_interface/config/pacmod.param.yaml b/pacmod_interface/config/pacmod.param.yaml index cc94ac4..560a343 100644 --- a/pacmod_interface/config/pacmod.param.yaml +++ b/pacmod_interface/config/pacmod.param.yaml @@ -19,3 +19,5 @@ accel_pedal_offset: 0.0 brake_pedal_offset: 0.0 margin_time_for_gear_change: 2.0 + enable_pub_steer: true + convert_steer_command: true diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index 89b6df8..e9d7788 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -159,6 +159,13 @@ class PacmodInterface : public rclcpp::Node double margin_time_for_gear_change_; // [s] + // steer command conversion + // if false, it is expected to be converted from and published actuation_status in + // raw_vehicle_cmd_converter + bool enable_pub_steer_ = true; // flag to publish steer_cmd + // if false, it is expected to be converted from and published actuation_status in + bool convert_steer_command_ = true; // flag to convert steer command + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // Service diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 5176b47..e1b0814 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -62,6 +62,10 @@ PacmodInterface::PacmodInterface() /* parameter for preventing gear chattering */ margin_time_for_gear_change_ = declare_parameter("margin_time_for_gear_change", 2.0); + /* parameter for steering conversion */ + enable_pub_steer_ = declare_parameter("enable_pub_steer", true); + convert_steer_command_ = declare_parameter("convert_steer_command", true); + /* initialize */ prev_steer_cmd_.header.stamp = this->now(); prev_steer_cmd_.command = 0.0; @@ -321,7 +325,7 @@ void PacmodInterface::callbackPacmodRpt( } /* publish current status */ - { + if (enable_pub_steer_) { autoware_vehicle_msgs::msg::SteeringReport steer_msg; steer_msg.stamp = header.stamp; steer_msg.steering_tire_angle = current_steer; @@ -330,11 +334,14 @@ void PacmodInterface::callbackPacmodRpt( /* publish control status */ { + // NOTE: + // actuation status must be the same data format as the input data. + // raw_vehicle_cmd_converter converts from steering_wheel_angle to steering_tire_angle. ActuationStatusStamped actuation_status; actuation_status.header = header; actuation_status.status.accel_status = accel_rpt_ptr_->output; actuation_status.status.brake_status = brake_rpt_ptr_->output; - actuation_status.status.steer_status = current_steer; + actuation_status.status.steer_status = current_steer_wheel; actuation_status_pub_->publish(actuation_status); } @@ -396,11 +403,17 @@ void PacmodInterface::publishCommands() const double current_steer_wheel = steer_wheel_rpt_ptr_->output; /* calculate desired steering wheel */ - double adaptive_gear_ratio = calculateVariableGearRatio(current_velocity, current_steer_wheel); - double desired_steer_wheel = - (control_cmd_ptr_->lateral.steering_tire_angle - steering_offset_) * adaptive_gear_ratio; - desired_steer_wheel = - std::min(std::max(desired_steer_wheel, -max_steering_wheel_), max_steering_wheel_); + const double adaptive_gear_ratio = + calculateVariableGearRatio(current_velocity, current_steer_wheel); + const double desired_steer_wheel = std::invoke([&]() -> double { + double desired_steer_wheel{0.0}; + if (convert_steer_command_) { + (control_cmd_ptr_->lateral.steering_tire_angle - steering_offset_) * adaptive_gear_ratio; + } else { + desired_steer_wheel = actuation_cmd_ptr_->actuation.steer_cmd; + } + return std::min(std::max(desired_steer_wheel, -max_steering_wheel_), max_steering_wheel_); + }); /* check clear flag */ bool clear_override = false; From df52c6a2eb830890e9d04a3abca8354fffb2740b Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 11 Sep 2024 15:23:54 +0900 Subject: [PATCH 2/4] fix Signed-off-by: kosuke55 --- pacmod_interface/src/pacmod_interface/pacmod_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index e1b0814..688041c 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -408,7 +408,8 @@ void PacmodInterface::publishCommands() const double desired_steer_wheel = std::invoke([&]() -> double { double desired_steer_wheel{0.0}; if (convert_steer_command_) { - (control_cmd_ptr_->lateral.steering_tire_angle - steering_offset_) * adaptive_gear_ratio; + desired_steer_wheel = + (control_cmd_ptr_->lateral.steering_tire_angle - steering_offset_) * adaptive_gear_ratio; } else { desired_steer_wheel = actuation_cmd_ptr_->actuation.steer_cmd; } From 1816618dcd5844113872cca4ec2aa00caa79ee98 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 24 Oct 2024 17:55:42 +0900 Subject: [PATCH 3/4] use steer angle of actuation cmd even when not convert_steer_command Signed-off-by: kosuke55 --- pacmod_interface/src/pacmod_interface/pacmod_interface.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 688041c..804b576 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -408,8 +408,10 @@ void PacmodInterface::publishCommands() const double desired_steer_wheel = std::invoke([&]() -> double { double desired_steer_wheel{0.0}; if (convert_steer_command_) { - desired_steer_wheel = - (control_cmd_ptr_->lateral.steering_tire_angle - steering_offset_) * adaptive_gear_ratio; + // NOTE: + // It is assumed that steer_cmd is send as actuation_cmd without being converted from + // raw_vehicle_cmd_converter. + desired_steer_wheel = (actuation.steer_cmd - steering_offset_) * adaptive_gear_ratio; } else { desired_steer_wheel = actuation_cmd_ptr_->actuation.steer_cmd; } From d5a6229e3511252e4d0bcf9e1bf4d0a7c8309705 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 31 Oct 2024 19:10:29 +0900 Subject: [PATCH 4/4] fix Signed-off-by: kosuke55 --- pacmod_interface/src/pacmod_interface/pacmod_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 804b576..19b8b18 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -411,7 +411,8 @@ void PacmodInterface::publishCommands() // NOTE: // It is assumed that steer_cmd is send as actuation_cmd without being converted from // raw_vehicle_cmd_converter. - desired_steer_wheel = (actuation.steer_cmd - steering_offset_) * adaptive_gear_ratio; + desired_steer_wheel = + (actuation_cmd_ptr_->actuation.steer_cmd - steering_offset_) * adaptive_gear_ratio; } else { desired_steer_wheel = actuation_cmd_ptr_->actuation.steer_cmd; }