From 492149c5cfa467b3ca78bb8a409c254c266389dc Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 7 Jul 2022 12:58:27 +0900 Subject: [PATCH] feat(vehicle cmd gate): add transition filter (#1244) * feat(vehicle_cmd_gate): add transition filter Signed-off-by: Takamasa Horibe * fix precommit Signed-off-by: Takamasa Horibe * remove debug code Signed-off-by: Takamasa Horibe * update param yaml Signed-off-by: Takamasa Horibe * update readme Signed-off-by: Takamasa Horibe * fix default topic name Signed-off-by: Takamasa Horibe --- control/vehicle_cmd_gate/CMakeLists.txt | 2 +- control/vehicle_cmd_gate/README.md | 35 ++++++----- .../config/vehicle_cmd_gate.param.yaml | 17 ++++-- .../vehicle_cmd_gate/vehicle_cmd_filter.hpp | 39 ++++++++---- .../vehicle_cmd_gate/vehicle_cmd_gate.hpp | 8 +++ control/vehicle_cmd_gate/package.xml | 1 + .../src/vehicle_cmd_filter.cpp | 45 ++++++++++---- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 59 +++++++++++++------ .../vehicle_cmd_gate.param.yaml | 17 ++++-- 9 files changed, 158 insertions(+), 65 deletions(-) diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt index a6f33e0591f48..4366fcdaaefce 100644 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ b/control/vehicle_cmd_gate/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.14) +cmake_minimum_required(VERSION 3.5) project(vehicle_cmd_gate) find_package(autoware_cmake REQUIRED) diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 5538182c9638f..45eae03cde7a2 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -26,6 +26,7 @@ | `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | | `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | | `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | +| `~/input/operation_mode` | `tier4_system_msgs::msg::OperationMode` | operation mode of Autoware | ### Output @@ -39,23 +40,29 @@ | `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | | `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | | `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | +| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | -------------------------------------------------------------- | -| `update_period` | double | update period | -| `use_emergency_handling` | bool | true when emergency handler is used | -| `use_external_emergency_stop` | bool | true when external emergency stop information is used | -| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | -| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | -| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | -| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | -| `vel_lim` | double | limit of longitudinal velocity | -| `lon_acc_lim` | double | limit of longitudinal acceleration | -| `lon_jerk_lim` | double | limit of longitudinal jerk | -| `lat_acc_lim` | double | limit of lateral acceleration | -| `lat_jerk_lim` | double | limit of lateral jerk | +| Parameter | Type | Description | +| ------------------------------------------- | ------ | --------------------------------------------------------------------------- | +| `update_period` | double | update period | +| `use_emergency_handling` | bool | true when emergency handler is used | +| `use_external_emergency_stop` | bool | true when external emergency stop information is used | +| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | +| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | +| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | +| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | +| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) | +| `nominal.lon_acc_lim` | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lon_jerk_lim` | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode) | +| `nominal.lat_acc_lim` | double | limit of lateral acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lat_jerk_lim` | double | limit of lateral jerk (activated in AUTONOMOUS operation mode) | +| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) | +| `on_transition.lon_acc_lim` | double | limit of longitudinal acceleration (activated in TRANSITION operation mode) | +| `on_transition.lon_jerk_lim` | double | limit of longitudinal jerk (activated in TRANSITION operation mode) | +| `on_transition.lat_acc_lim` | double | limit of lateral acceleration (activated in TRANSITION operation mode) | +| `on_transition.lat_jerk_lim` | double | limit of lateral jerk (activated in TRANSITION operation mode) | ## Assumptions / Known limits diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 0ad4631c6df53..4876699351ffd 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -6,8 +6,15 @@ stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 stopped_state_entry_duration_time: 0.1 - vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 + nominal: + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 + on_transition: + vel_lim: 50.0 + lon_acc_lim: 1.0 + lon_jerk_lim: 0.5 + lat_acc_lim: 1.2 + lat_jerk_lim: 0.75 diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp index b72dff42572f7..5562240a35c49 100644 --- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp @@ -15,20 +15,34 @@ #ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ #define VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ +#include + #include +struct VehicleCmdFilterParam +{ + double wheel_base; + double vel_lim; + double lon_acc_lim; + double lon_jerk_lim; + double lat_acc_lim; + double lat_jerk_lim; + double actual_steer_diff_lim; +}; class VehicleCmdFilter { public: VehicleCmdFilter(); ~VehicleCmdFilter() = default; - void setWheelBase(double v) { wheel_base_ = v; } - void setVelLim(double v) { vel_lim_ = v; } - void setLonAccLim(double v) { lon_acc_lim_ = v; } - void setLonJerkLim(double v) { lon_jerk_lim_ = v; } - void setLatAccLim(double v) { lat_acc_lim_ = v; } - void setLatJerkLim(double v) { lat_jerk_lim_ = v; } + void setWheelBase(double v) { param_.wheel_base = v; } + void setVelLim(double v) { param_.vel_lim = v; } + void setLonAccLim(double v) { param_.lon_acc_lim = v; } + void setLonJerkLim(double v) { param_.lon_jerk_lim = v; } + void setLatAccLim(double v) { param_.lat_acc_lim = v; } + void setLatJerkLim(double v) { param_.lat_jerk_lim = v; } + void setActualSteerDiffLim(double v) { param_.actual_steer_diff_lim = v; } + void setParam(const VehicleCmdFilterParam & p) { param_ = p; } void setPrevCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand & v) { prev_cmd_ = v; @@ -44,14 +58,15 @@ class VehicleCmdFilter const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; void limitLateralWithLatJerk( const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void limitActualSteerDiff( + const double current_steer_angle, + autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void filterAll( + const double dt, const double current_steer_angle, + autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; private: - double wheel_base_; - double vel_lim_; - double lon_acc_lim_; - double lon_jerk_lim_; - double lat_acc_lim_; - double lat_jerk_lim_; + VehicleCmdFilterParam param_; autoware_auto_control_msgs::msg::AckermannControlCommand prev_cmd_; double calcLatAcc(const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const; diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp index 14e403e4e9896..3af3a5e8814e0 100644 --- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -53,6 +54,7 @@ using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; +using tier4_system_msgs::msg::OperationMode; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; using diagnostic_msgs::msg::DiagnosticStatus; @@ -87,12 +89,14 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; rclcpp::Publisher::SharedPtr gate_mode_pub_; rclcpp::Publisher::SharedPtr engage_pub_; + rclcpp::Publisher::SharedPtr operation_mode_pub_; // Subscription rclcpp::Subscription::SharedPtr emergency_state_sub_; rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; rclcpp::Subscription::SharedPtr gate_mode_sub_; rclcpp::Subscription::SharedPtr steer_sub_; + rclcpp::Subscription::SharedPtr operation_mode_sub_; void onGateMode(GateMode::ConstSharedPtr msg); void onEmergencyState(EmergencyState::ConstSharedPtr msg); @@ -208,6 +212,10 @@ class VehicleCmdGate : public rclcpp::Node VehicleCmdFilter filter_; AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg); + // filtering on transition + OperationMode current_operation_mode_; + VehicleCmdFilter filter_on_transition_; + // Start request service struct StartRequest { diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index 7cb8b18244d18..f2bb7733a3c61 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -23,6 +23,7 @@ tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs + tier4_system_msgs tier4_vehicle_msgs vehicle_info_util diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 42aa3111caef7..41ed6c9aa62fe 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -21,24 +21,25 @@ VehicleCmdFilter::VehicleCmdFilter() {} void VehicleCmdFilter::limitLongitudinalWithVel( autoware_auto_control_msgs::msg::AckermannControlCommand & input) const { - input.longitudinal.speed = - std::max(std::min(static_cast(input.longitudinal.speed), vel_lim_), -vel_lim_); + input.longitudinal.speed = std::max( + std::min(static_cast(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim); } void VehicleCmdFilter::limitLongitudinalWithAcc( const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const { input.longitudinal.acceleration = std::max( - std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim_), -lon_acc_lim_); + std::min(static_cast(input.longitudinal.acceleration), param_.lon_acc_lim), + -param_.lon_acc_lim); input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim_ * dt); + limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, param_.lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const { input.longitudinal.acceleration = limitDiff( - input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, lon_jerk_lim_ * dt); + input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt); } void VehicleCmdFilter::limitLateralWithLatAcc( @@ -46,10 +47,10 @@ void VehicleCmdFilter::limitLateralWithLatAcc( autoware_auto_control_msgs::msg::AckermannControlCommand & input) const { double latacc = calcLatAcc(input); - if (std::fabs(latacc) > lat_acc_lim_) { + if (std::fabs(latacc) > param_.lat_acc_lim) { double v_sq = std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); - double steer_lim = std::atan(lat_acc_lim_ * wheel_base_ / v_sq); + double steer_lim = std::atan(param_.lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } @@ -60,8 +61,8 @@ void VehicleCmdFilter::limitLateralWithLatJerk( double curr_latacc = calcLatAcc(input); double prev_latacc = calcLatAcc(prev_cmd_); - const double latacc_max = prev_latacc + lat_jerk_lim_ * dt; - const double latacc_min = prev_latacc - lat_jerk_lim_ * dt; + const double latacc_max = prev_latacc + param_.lat_jerk_lim * dt; + const double latacc_min = prev_latacc - param_.lat_jerk_lim * dt; if (curr_latacc > latacc_max) { input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); @@ -70,17 +71,39 @@ void VehicleCmdFilter::limitLateralWithLatJerk( } } +void VehicleCmdFilter::limitActualSteerDiff( + const double current_steer_angle, + autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +{ + auto ds = input.lateral.steering_tire_angle - current_steer_angle; + ds = std::clamp(ds, -param_.actual_steer_diff_lim, param_.actual_steer_diff_lim); + input.lateral.steering_tire_angle = current_steer_angle + ds; +} + +void VehicleCmdFilter::filterAll( + const double dt, const double current_steer_angle, + autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const +{ + limitLongitudinalWithJerk(dt, cmd); + limitLongitudinalWithAcc(dt, cmd); + limitLongitudinalWithVel(cmd); + limitLateralWithLatJerk(dt, cmd); + limitLateralWithLatAcc(dt, cmd); + limitActualSteerDiff(current_steer_angle, cmd); + return; +} + double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc) const { const double v_sq = std::max(v * v, 0.001); - return std::atan(latacc * wheel_base_ / v_sq); + return std::atan(latacc * param_.wheel_base / v_sq); } double VehicleCmdFilter::calcLatAcc( const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const { double v = cmd.longitudinal.speed; - return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheel_base_; + return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } double VehicleCmdFilter::limitDiff( diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 05dc01190e6f3..66b0f8ea7c03f 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -66,6 +66,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_pub_ = this->create_publisher("output/engage", durable_qos); pub_external_emergency_ = this->create_publisher("output/external_emergency", durable_qos); + operation_mode_pub_ = this->create_publisher("output/operation_mode", durable_qos); // Subscriber emergency_state_sub_ = this->create_subscription( @@ -79,6 +80,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); steer_sub_ = this->create_subscription( "input/steering", 1, std::bind(&VehicleCmdGate::onSteering, this, _1)); + operation_mode_sub_ = this->create_subscription( + "input/operation_mode", 1, [this](const tier4_system_msgs::msg::OperationMode::SharedPtr msg) { + current_operation_mode_ = *msg; + }); // Subscriber for auto auto_control_cmd_sub_ = this->create_subscription( @@ -133,18 +138,29 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - double wheel_base = vehicle_info.wheel_base_m; - double vel_lim = declare_parameter("vel_lim", 25.0); - double lon_acc_lim = declare_parameter("lon_acc_lim", 5.0); - double lon_jerk_lim = declare_parameter("lon_jerk_lim", 5.0); - double lat_acc_lim = declare_parameter("lat_acc_lim", 5.0); - double lat_jerk_lim = declare_parameter("lat_jerk_lim", 5.0); - filter_.setWheelBase(wheel_base); - filter_.setVelLim(vel_lim); - filter_.setLonAccLim(lon_acc_lim); - filter_.setLonJerkLim(lon_jerk_lim); - filter_.setLatAccLim(lat_acc_lim); - filter_.setLatJerkLim(lat_jerk_lim); + { + VehicleCmdFilterParam p; + p.wheel_base = vehicle_info.wheel_base_m; + p.vel_lim = declare_parameter("nominal.vel_lim", 25.0); + p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim", 5.0); + p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim", 5.0); + p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim", 5.0); + p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim", 5.0); + p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim", 1.0); + filter_.setParam(p); + } + + { + VehicleCmdFilterParam p; + p.wheel_base = vehicle_info.wheel_base_m; + p.vel_lim = declare_parameter("on_transition.vel_lim", 25.0); + p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim", 0.5); + p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim", 0.25); + p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim", 0.5); + p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim", 0.25); + p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim", 0.05); + filter_on_transition_.setParam(p); + } // Set default value current_gate_mode_.data = GateMode::AUTO; @@ -485,6 +501,7 @@ void VehicleCmdGate::publishStatus() gate_mode_pub_->publish(current_gate_mode_); engage_pub_->publish(autoware_engage); pub_external_emergency_->publish(external_emergency); + operation_mode_pub_->publish(current_operation_mode_); } AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in) @@ -492,12 +509,20 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont AckermannControlCommand out = in; const double dt = getDt(); - filter_.limitLongitudinalWithVel(out); - filter_.limitLongitudinalWithAcc(dt, out); - filter_.limitLongitudinalWithJerk(dt, out); - filter_.limitLateralWithLatAcc(dt, out); - filter_.limitLateralWithLatJerk(dt, out); + const auto mode = current_operation_mode_.mode; + + // Apply transition_filter when transiting from MANUAL to AUTO. + if (mode == OperationMode::TRANSITION_TO_AUTO) { + filter_on_transition_.filterAll(dt, current_steer_, out); + } else { + filter_.filterAll(dt, current_steer_, out); + } + + // set prev value for both to keep consistency over switching + // TODO(Horibe): prev value should be actual steer, vel, acc when Manual mode to keep + // consistency when switching from Manual to Auto. filter_.setPrevCmd(out); + filter_on_transition_.setPrevCmd(out); return out; } diff --git a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 0ad4631c6df53..4876699351ffd 100644 --- a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -6,8 +6,15 @@ stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 stopped_state_entry_duration_time: 0.1 - vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 + nominal: + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 + on_transition: + vel_lim: 50.0 + lon_acc_lim: 1.0 + lon_jerk_lim: 0.5 + lat_acc_lim: 1.2 + lat_jerk_lim: 0.75