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_