Skip to content

Commit 84786ed

Browse files
RBT22masf7g
RBT22
authored and
masf7g
committed
Add acceleration limits to DriveOnHeading and BackUp behaviors (ros-navigation#4810)
* Add acceleration constraints Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Cleanup code Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Format code Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add <limits> header to drive_on_heading.hpp Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Remove vel pointer Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Use the limits only if both of them is set Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Fix onActionCompletion params Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add default acc params and change decel sign Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add minimum speed parameter Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Update minimum speed parameter to 0.10 Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Log warning when acceleration or deceleration limits are not set Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add param sign assert Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Remove unnecessary param checking Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Refactor acceleration limits to handle forward and backward movement separately Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Fix sign checking condition Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Replace throwing with silent sign correction Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com>
1 parent 2ba4da1 commit 84786ed

File tree

2 files changed

+64
-1
lines changed

2 files changed

+64
-1
lines changed

‎nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp‎

Lines changed: 58 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <chrono>
2020
#include <memory>
2121
#include <utility>
22+
#include <limits>
2223

2324
#include "nav2_behaviors/timed_behavior.hpp"
2425
#include "nav2_msgs/action/drive_on_heading.hpp"
@@ -133,7 +134,30 @@ class DriveOnHeading : public TimedBehavior<ActionT>
133134
cmd_vel->header.frame_id = this->robot_base_frame_;
134135
cmd_vel->twist.linear.y = 0.0;
135136
cmd_vel->twist.angular.z = 0.0;
136-
cmd_vel->twist.linear.x = command_speed_;
137+
138+
double current_speed = last_vel_ == std::numeric_limits<double>::max() ? 0.0 : last_vel_;
139+
bool forward = command_speed_ > 0.0;
140+
double min_feasible_speed, max_feasible_speed;
141+
if (forward) {
142+
min_feasible_speed = current_speed + deceleration_limit_ / this->cycle_frequency_;
143+
max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_;
144+
} else {
145+
min_feasible_speed = current_speed - acceleration_limit_ / this->cycle_frequency_;
146+
max_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_;
147+
}
148+
cmd_vel->twist.linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed);
149+
150+
// Check if we need to slow down to avoid overshooting
151+
auto remaining_distance = std::fabs(command_x_) - distance;
152+
double max_vel_to_stop = std::sqrt(-2.0 * deceleration_limit_ * remaining_distance);
153+
if (max_vel_to_stop < std::abs(cmd_vel->twist.linear.x)) {
154+
cmd_vel->twist.linear.x = forward ? max_vel_to_stop : -max_vel_to_stop;
155+
}
156+
157+
// Ensure we don't go below minimum speed
158+
if (std::fabs(cmd_vel->twist.linear.x) < minimum_speed_) {
159+
cmd_vel->twist.linear.x = forward ? minimum_speed_ : -minimum_speed_;
160+
}
137161

138162
geometry_msgs::msg::Pose2D pose2d;
139163
pose2d.x = current_pose.pose.position.x;
@@ -146,6 +170,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
146170
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
147171
}
148172

173+
last_vel_ = cmd_vel->twist.linear.x;
149174
this->vel_pub_->publish(std::move(cmd_vel));
150175

151176
return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
@@ -157,6 +182,14 @@ class DriveOnHeading : public TimedBehavior<ActionT>
157182
*/
158183
CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
159184

185+
void onCleanup() override {last_vel_ = std::numeric_limits<double>::max();}
186+
187+
void onActionCompletion(std::shared_ptr<typename ActionT::Result>/*result*/)
188+
override
189+
{
190+
last_vel_ = std::numeric_limits<double>::max();
191+
}
192+
160193
protected:
161194
/**
162195
* @brief Check if pose is collision free
@@ -228,6 +261,26 @@ class DriveOnHeading : public TimedBehavior<ActionT>
228261
node,
229262
"simulate_ahead_time", rclcpp::ParameterValue(2.0));
230263
node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
264+
265+
nav2_util::declare_parameter_if_not_declared(
266+
node, this->behavior_name_ + ".acceleration_limit",
267+
rclcpp::ParameterValue(2.5));
268+
nav2_util::declare_parameter_if_not_declared(
269+
node, this->behavior_name_ + ".deceleration_limit",
270+
rclcpp::ParameterValue(-2.5));
271+
nav2_util::declare_parameter_if_not_declared(
272+
node, this->behavior_name_ + ".minimum_speed",
273+
rclcpp::ParameterValue(0.10));
274+
node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_);
275+
node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
276+
node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_);
277+
if (acceleration_limit_ <= 0.0 || deceleration_limit_ >= 0.0) {
278+
RCLCPP_ERROR(this->logger_,
279+
"DriveOnHeading: acceleration_limit and deceleration_limit must be "
280+
"positive and negative respectively");
281+
acceleration_limit_ = std::abs(acceleration_limit_);
282+
deceleration_limit_ = -std::abs(deceleration_limit_);
283+
}
231284
}
232285

233286
typename ActionT::Feedback::SharedPtr feedback_;
@@ -239,6 +292,10 @@ class DriveOnHeading : public TimedBehavior<ActionT>
239292
rclcpp::Duration command_time_allowance_{0, 0};
240293
rclcpp::Time end_time_;
241294
double simulate_ahead_time_;
295+
double acceleration_limit_;
296+
double deceleration_limit_;
297+
double minimum_speed_;
298+
double last_vel_ = std::numeric_limits<double>::max();
242299
};
243300

244301
} // namespace nav2_behaviors

‎nav2_bringup/params/nav2_params.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -305,8 +305,14 @@ behavior_server:
305305
plugin: "nav2_behaviors::Spin"
306306
backup:
307307
plugin: "nav2_behaviors::BackUp"
308+
acceleration_limit: 2.5
309+
deceleration_limit: -2.5
310+
minimum_speed: 0.10
308311
drive_on_heading:
309312
plugin: "nav2_behaviors::DriveOnHeading"
313+
acceleration_limit: 2.5
314+
deceleration_limit: -2.5
315+
minimum_speed: 0.10
310316
wait:
311317
plugin: "nav2_behaviors::Wait"
312318
assisted_teleop:

0 commit comments

Comments
 (0)