From 065068bc5b6b89cf38b40ed91dce83d5a1b45ed9 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 4 Mar 2024 17:33:52 +0900 Subject: [PATCH] fix(manual_controller): set PARK gear when going from reverse to drive (#6230) Signed-off-by: Maxime CLEMENT --- .../src/tools/manual_controller.cpp | 54 +++++++------------ .../src/tools/manual_controller.hpp | 7 +-- 2 files changed, 22 insertions(+), 39 deletions(-) diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index fdd270fcce2ef..8bbb096f728ec 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -30,14 +30,6 @@ using std::placeholders::_1; namespace rviz_plugins { -double lowpassFilter( - const double current_value, const double prev_value, double cutoff, const double dt) -{ - const double tau = 1.0 / (2.0 * M_PI * cutoff); - const double a = tau / (dt + tau); - return prev_value * a + (1.0 - a) * current_value; -} - ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent) { auto * state_layout = new QHBoxLayout; @@ -115,25 +107,23 @@ void ManualController::update() ackermann.stamp = raw_node_->get_clock()->now(); ackermann.lateral.steering_tire_angle = steering_angle_; ackermann.longitudinal.speed = cruise_velocity_; - if (current_acceleration_) { - /** - * @brief Calculate desired acceleration by simple BackSteppingControl - * V = 0.5*(v-v_des)^2 >= 0 - * D[V] = (D[v] - a_des)*(v-v_des) <=0 - * a_des = k_const *(v - v_des) + a (k < 0 ) - */ - const double k = -0.5; - const double v = current_velocity_; - const double v_des = cruise_velocity_; - const double a = *current_acceleration_; - const double a_des = k * (v - v_des) + a; - ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); - } + /** + * @brief Calculate desired acceleration by simple BackSteppingControl + * V = 0.5*(v-v_des)^2 >= 0 + * D[V] = (D[v] - a_des)*(v-v_des) <=0 + * a_des = k_const *(v - v_des) + a (k < 0 ) + */ + const double k = -0.5; + const double v = current_velocity_; + const double v_des = cruise_velocity_; + const double a = current_acceleration_; + const double a_des = k * (v - v_des) + a; + ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); } GearCommand gear_cmd; { const double eps = 0.001; - if (ackermann.longitudinal.speed > eps) { + if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) { gear_cmd.command = GearCommand::DRIVE; } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) { gear_cmd.command = GearCommand::REVERSE; @@ -220,19 +210,11 @@ void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg) void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg) { current_velocity_ = msg->longitudinal_velocity; - if (previous_velocity_) { - const double cutoff = 10.0; - const double dt = 1.0 / 10.0; - const double acc = (current_velocity_ - *previous_velocity_) / dt; - if (!current_acceleration_) { - current_acceleration_ = std::make_unique(acc); - } else { - current_acceleration_ = - std::make_unique(lowpassFilter(acc, *current_acceleration_, cutoff, dt)); - } - } - previous_velocity_ = std::make_unique(msg->longitudinal_velocity); - prev_stamp_ = rclcpp::Time(msg->header.stamp); +} + +void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + current_acceleration_ = msg->accel.accel.linear.x; } void ManualController::onGear(const GearReport::ConstSharedPtr msg) diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp index dee6f9a7aba21..aaa625bff911e 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -25,6 +25,7 @@ #include #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include #include @@ -40,6 +41,7 @@ namespace rviz_plugins using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using tier4_control_msgs::msg::GateMode; using EngageSrv = tier4_external_api_msgs::srv::Engage; @@ -67,6 +69,7 @@ public Q_SLOTS: // NOLINT for Qt void onPublishControlCommand(); void onGateMode(const GateMode::ConstSharedPtr msg); void onVelocity(const VelocityReport::ConstSharedPtr msg); + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onEngageStatus(const Engage::ConstSharedPtr msg); void onGear(const GearReport::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; @@ -82,9 +85,7 @@ public Q_SLOTS: // NOLINT for Qt double cruise_velocity_{0.0}; double steering_angle_{0.0}; double current_velocity_{0.0}; - rclcpp::Time prev_stamp_; - std::unique_ptr previous_velocity_; - std::unique_ptr current_acceleration_; + double current_acceleration_{0.0}; QLabel * gate_mode_label_ptr_; QLabel * gear_label_ptr_;