Merge pull request #57 from booars/chore/cherry-pick-upstream-pr143

chore: cherry pick upstream PR143

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml

@@ -4,6 +4,7 @@
   <arg name="csv_path_brake_map" default="$(find-pkg-share aichallenge_submit_launch)/data/brake_map.csv"/>
 
   <node pkg="aichallenge_awsim_adapter" exec="actuation_cmd_converter">
+    <param name="steer_delay_sec" value="0.2"/>
     <param name="csv_path_accel_map" value="$(var csv_path_accel_map)"/>
     <param name="csv_path_brake_map" value="$(var csv_path_brake_map)"/>
   </node>

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp

@@ -22,7 +22,8 @@ ActuationCmdConverter::ActuationCmdConverter(const rclcpp::NodeOptions & node_op
   // Parameters
   const std::string csv_path_accel_map = declare_parameter<std::string>("csv_path_accel_map");
   const std::string csv_path_brake_map = declare_parameter<std::string>("csv_path_brake_map");
-
+  steer_delay_sec_ = this->declare_parameter<double>("steer_delay_sec");
+  delay_ = std::chrono::duration<double>(steer_delay_sec_);
   // Subscriptions
   sub_actuation_ = create_subscription<ActuationCommandStamped>(
     "/control/command/actuation_cmd", 1, std::bind(&ActuationCmdConverter::on_actuation_cmd, this, _1));
@@ -64,12 +65,15 @@ void ActuationCmdConverter::on_actuation_cmd(const ActuationCommandStamped::Cons
 
   const double velocity = std::abs(velocity_report_->longitudinal_velocity);
   const double acceleration = get_acceleration(*msg, velocity);
+  // Add steer_cmd to history. Limit -35 deg to 35 deg
+  steer_cmd_history_.emplace_back(msg->header.stamp, std::clamp(msg->actuation.steer_cmd, -0.61, 0.61));
+
 
   // Publish ControlCommand
   constexpr float nan = std::numeric_limits<double>::quiet_NaN();
   AckermannControlCommand output;
   output.stamp = msg->header.stamp;
-  output.lateral.steering_tire_angle = static_cast<float>(msg->actuation.steer_cmd);
+  output.lateral.steering_tire_angle = get_delayed_steer_cmd(msg->header.stamp);
   output.lateral.steering_tire_rotation_rate = nan;
   output.longitudinal.speed = nan;
   output.longitudinal.acceleration = static_cast<float>(acceleration);
@@ -88,5 +92,15 @@ double ActuationCmdConverter::get_acceleration(const ActuationCommandStamped & c
   return ref_acceleration;
 }
 
+double ActuationCmdConverter::get_delayed_steer_cmd(const rclcpp::Time& current_time)
+{
+  // Delete old steer_cmd
+  while (!steer_cmd_history_.empty() && 
+         (current_time - steer_cmd_history_.front().first).seconds() > delay_.count())
+  {
+    steer_cmd_history_.pop_front();
+  }
+  return steer_cmd_history_.empty() ? 0.0 : steer_cmd_history_.front().second;
+}
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(ActuationCmdConverter)

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp

@@ -15,6 +15,8 @@
 #ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_
 #define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_
 
+#include <deque>
+
 #include <raw_vehicle_cmd_converter/accel_map.hpp>
 #include <raw_vehicle_cmd_converter/brake_map.hpp>
 
@@ -50,6 +52,13 @@ class ActuationCmdConverter : public rclcpp::Node
   raw_vehicle_cmd_converter::BrakeMap brake_map_;
   GearReport::ConstSharedPtr gear_report_;
   VelocityReport::ConstSharedPtr velocity_report_;
+
+  std::deque<std::pair<rclcpp::Time, double>> steer_cmd_history_;
+  // delay for steering command
+  std::chrono::duration<double> delay_; 
+  double get_delayed_steer_cmd(const rclcpp::Time& current_time);
+  double steer_delay_sec_;
+
 };
 
 #endif  // AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_