diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml
index dc0b8ab6..fe861572 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml
@@ -4,6 +4,7 @@
+
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp
index fc911d3d..c2ac6c03 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp
+++ b/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("csv_path_accel_map");
const std::string csv_path_brake_map = declare_parameter("csv_path_brake_map");
-
+ steer_delay_sec_ = this->declare_parameter("steer_delay_sec");
+ delay_ = std::chrono::duration(steer_delay_sec_);
// Subscriptions
sub_actuation_ = create_subscription(
"/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::quiet_NaN();
AckermannControlCommand output;
output.stamp = msg->header.stamp;
- output.lateral.steering_tire_angle = static_cast(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(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(ActuationCmdConverter)
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp
index 06e0f5e7..cf0c7cba 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp
+++ b/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
+
#include
#include
@@ -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> steer_cmd_history_;
+ // delay for steering command
+ std::chrono::duration delay_;
+ double get_delayed_steer_cmd(const rclcpp::Time& current_time);
+ double steer_delay_sec_;
+
};
#endif // AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_