feat(pacmod_interface): Additional option to receive wheel angle dire…

…ctly

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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

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 desired_steer_wheel = std::invoke([&]() -> double {
+    const double adaptive_gear_ratio =
+      calculateVariableGearRatio(current_velocity, current_steer_wheel);
+    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;