fix(manual_controller): set PARK gear when going from reverse to drive

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

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<double>(acc);
-    } else {
-      current_acceleration_ =
-        std::make_unique<double>(lowpassFilter(acc, *current_acceleration_, cutoff, dt));
-    }
-  }
-  previous_velocity_ = std::make_unique<double>(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)

common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp

@@ -25,6 +25,7 @@
 #include <rviz_common/panel.hpp>
 
 #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 <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/engage.hpp>
@@ -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<double> previous_velocity_;
-  std::unique_ptr<double> current_acceleration_;
+  double current_acceleration_{0.0};
 
   QLabel * gate_mode_label_ptr_;
   QLabel * gear_label_ptr_;