@@ -30,14 +30,6 @@ using std::placeholders::_1;
3030namespace 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-
4133ManualController::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)
220210void 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
238220void ManualController::onGear (const GearReport::ConstSharedPtr msg)
0 commit comments