Skip to content

Commit 332e084

Browse files
committed
fix(manual_controller): set PARK gear when going from reverse to drive
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 47f265d commit 332e084

File tree

2 files changed

+22
-39
lines changed

2 files changed

+22
-39
lines changed

common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

+18-36
Original file line numberDiff line numberDiff line change
@@ -30,14 +30,6 @@ using std::placeholders::_1;
3030
namespace rviz_plugins
3131
{
3232

33-
double lowpassFilter(
34-
const double current_value, const double prev_value, double cutoff, const double dt)
35-
{
36-
const double tau = 1.0 / (2.0 * M_PI * cutoff);
37-
const double a = tau / (dt + tau);
38-
return prev_value * a + (1.0 - a) * current_value;
39-
}
40-
4133
ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent)
4234
{
4335
auto * state_layout = new QHBoxLayout;
@@ -115,25 +107,23 @@ void ManualController::update()
115107
ackermann.stamp = raw_node_->get_clock()->now();
116108
ackermann.lateral.steering_tire_angle = steering_angle_;
117109
ackermann.longitudinal.speed = cruise_velocity_;
118-
if (current_acceleration_) {
119-
/**
120-
* @brief Calculate desired acceleration by simple BackSteppingControl
121-
* V = 0.5*(v-v_des)^2 >= 0
122-
* D[V] = (D[v] - a_des)*(v-v_des) <=0
123-
* a_des = k_const *(v - v_des) + a (k < 0 )
124-
*/
125-
const double k = -0.5;
126-
const double v = current_velocity_;
127-
const double v_des = cruise_velocity_;
128-
const double a = *current_acceleration_;
129-
const double a_des = k * (v - v_des) + a;
130-
ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0);
131-
}
110+
/**
111+
* @brief Calculate desired acceleration by simple BackSteppingControl
112+
* V = 0.5*(v-v_des)^2 >= 0
113+
* D[V] = (D[v] - a_des)*(v-v_des) <=0
114+
* a_des = k_const *(v - v_des) + a (k < 0 )
115+
*/
116+
const double k = -0.5;
117+
const double v = current_velocity_;
118+
const double v_des = cruise_velocity_;
119+
const double a = current_acceleration_;
120+
const double a_des = k * (v - v_des) + a;
121+
ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0);
132122
}
133123
GearCommand gear_cmd;
134124
{
135125
const double eps = 0.001;
136-
if (ackermann.longitudinal.speed > eps) {
126+
if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) {
137127
gear_cmd.command = GearCommand::DRIVE;
138128
} else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) {
139129
gear_cmd.command = GearCommand::REVERSE;
@@ -220,19 +210,11 @@ void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg)
220210
void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg)
221211
{
222212
current_velocity_ = msg->longitudinal_velocity;
223-
if (previous_velocity_) {
224-
const double cutoff = 10.0;
225-
const double dt = 1.0 / 10.0;
226-
const double acc = (current_velocity_ - *previous_velocity_) / dt;
227-
if (!current_acceleration_) {
228-
current_acceleration_ = std::make_unique<double>(acc);
229-
} else {
230-
current_acceleration_ =
231-
std::make_unique<double>(lowpassFilter(acc, *current_acceleration_, cutoff, dt));
232-
}
233-
}
234-
previous_velocity_ = std::make_unique<double>(msg->longitudinal_velocity);
235-
prev_stamp_ = rclcpp::Time(msg->header.stamp);
213+
}
214+
215+
void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
216+
{
217+
current_acceleration_ = msg->accel.accel.linear.x;
236218
}
237219

238220
void ManualController::onGear(const GearReport::ConstSharedPtr msg)

common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include <rviz_common/panel.hpp>
2626

2727
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
28+
#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
2829
#include "geometry_msgs/msg/twist.hpp"
2930
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
3031
#include <autoware_auto_vehicle_msgs/msg/engage.hpp>
@@ -40,6 +41,7 @@ namespace rviz_plugins
4041
using autoware_auto_control_msgs::msg::AckermannControlCommand;
4142
using autoware_auto_vehicle_msgs::msg::GearCommand;
4243
using autoware_auto_vehicle_msgs::msg::VelocityReport;
44+
using geometry_msgs::msg::AccelWithCovarianceStamped;
4345
using geometry_msgs::msg::Twist;
4446
using tier4_control_msgs::msg::GateMode;
4547
using EngageSrv = tier4_external_api_msgs::srv::Engage;
@@ -67,6 +69,7 @@ public Q_SLOTS: // NOLINT for Qt
6769
void onPublishControlCommand();
6870
void onGateMode(const GateMode::ConstSharedPtr msg);
6971
void onVelocity(const VelocityReport::ConstSharedPtr msg);
72+
void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
7073
void onEngageStatus(const Engage::ConstSharedPtr msg);
7174
void onGear(const GearReport::ConstSharedPtr msg);
7275
rclcpp::Node::SharedPtr raw_node_;
@@ -82,9 +85,7 @@ public Q_SLOTS: // NOLINT for Qt
8285
double cruise_velocity_{0.0};
8386
double steering_angle_{0.0};
8487
double current_velocity_{0.0};
85-
rclcpp::Time prev_stamp_;
86-
std::unique_ptr<double> previous_velocity_;
87-
std::unique_ptr<double> current_acceleration_;
88+
double current_acceleration_{0.0};
8889

8990
QLabel * gate_mode_label_ptr_;
9091
QLabel * gear_label_ptr_;

0 commit comments

Comments
 (0)