Skip to content

Commit 76b76c9

Browse files
RBT22LinusTxtonomySteveMacenskipadhupradheepThomasHaley-neya
authored
Nav2 Sync 6 2025 Febr 4 (#53)
* Update Smac Planner for rounding to closest bin rather than flooring (ros-navigation#4636) (ros-navigation#4760) Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * fixing path longer on approach (ros-navigation#4622) (ros-navigation#4766) * fixing path longer on approach * removing the short circuit * adding additional layer of check --------- Signed-off-by: Pradheep <padhupradheep@gmail.com> * Add acceleration limits to DriveOnHeading and BackUp behaviors (backport ros-navigation#4810) (ros-navigation#4877) * 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> * Update parameter defaults to zero Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add off condition Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Move forward outside Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add option to use open-loop control with Rotation Shim (backport ros-navigation#4880) (ros-navigation#4896) * Add option to use open-loop control with Rotation Shim (ros-navigation#4880) * Initial implementation Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * replace feedback param with closed_loop Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Reset last_angular_vel_ in activate method Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add closed_loop parameter to dynamicParametersCallback Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add tests Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Override reset function Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Remove reset Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Create footprint_collision_checker.py (ros-navigation#4899) Backport of footprint_collision_checker to Nav2 Humble Signed-off-by: ThomasHaley-neya <thaley@neyarobotics.com> --------- Signed-off-by: Pradheep <padhupradheep@gmail.com> Signed-off-by: RBT22 <rozgonyibalint@gmail.com> Signed-off-by: ThomasHaley-neya <thaley@neyarobotics.com> Co-authored-by: LinusTxtonomy <152272158+LinusTxtonomy@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com> Co-authored-by: ThomasHaley-neya <thaley@neyarobotics.com>
1 parent e02a144 commit 76b76c9

File tree

9 files changed

+409
-28
lines changed

9 files changed

+409
-28
lines changed

‎nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp‎

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -34,9 +34,10 @@ bool PathLongerOnApproach::isPathUpdated(
3434
nav_msgs::msg::Path & new_path,
3535
nav_msgs::msg::Path & old_path)
3636
{
37-
return new_path != old_path && old_path.poses.size() != 0 &&
37+
return old_path.poses.size() != 0 &&
3838
new_path.poses.size() != 0 &&
39-
old_path.poses.back().pose == new_path.poses.back().pose;
39+
new_path.poses.size() != old_path.poses.size() &&
40+
old_path.poses.back().pose.position == new_path.poses.back().pose.position;
4041
}
4142

4243
bool PathLongerOnApproach::isRobotInGoalProximity(
@@ -62,10 +63,12 @@ inline BT::NodeStatus PathLongerOnApproach::tick()
6263
getInput("prox_len", prox_len_);
6364
getInput("length_factor", length_factor_);
6465

65-
if (status() == BT::NodeStatus::IDLE) {
66-
// Reset the starting point since we're starting a new iteration of
67-
// PathLongerOnApproach (moving from IDLE to RUNNING)
68-
first_time_ = true;
66+
if (first_time_ == false) {
67+
if (old_path_.poses.empty() || new_path_.poses.empty() ||
68+
old_path_.poses.back().pose != new_path_.poses.back().pose)
69+
{
70+
first_time_ = true;
71+
}
6972
}
7073

7174
setStatus(BT::NodeStatus::RUNNING);
@@ -78,13 +81,12 @@ inline BT::NodeStatus PathLongerOnApproach::tick()
7881
const BT::NodeStatus child_state = child_node_->executeTick();
7982
switch (child_state) {
8083
case BT::NodeStatus::RUNNING:
81-
return BT::NodeStatus::RUNNING;
84+
return child_state;
8285
case BT::NodeStatus::SUCCESS:
83-
old_path_ = new_path_;
84-
return BT::NodeStatus::SUCCESS;
8586
case BT::NodeStatus::FAILURE:
8687
old_path_ = new_path_;
87-
return BT::NodeStatus::FAILURE;
88+
resetChild();
89+
return child_state;
8890
default:
8991
old_path_ = new_path_;
9092
return BT::NodeStatus::FAILURE;

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

Lines changed: 62 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"
@@ -126,7 +127,35 @@ class DriveOnHeading : public TimedBehavior<ActionT>
126127
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
127128
cmd_vel->linear.y = 0.0;
128129
cmd_vel->angular.z = 0.0;
129-
cmd_vel->linear.x = command_speed_;
130+
131+
bool forward = command_speed_ > 0.0;
132+
if (acceleration_limit_ == 0.0 || deceleration_limit_ == 0.0) {
133+
RCLCPP_INFO_ONCE(this->logger_, "DriveOnHeading: no acceleration or deceleration limits set");
134+
cmd_vel->linear.x = command_speed_;
135+
} else {
136+
double current_speed = last_vel_ == std::numeric_limits<double>::max() ? 0.0 : last_vel_;
137+
double min_feasible_speed, max_feasible_speed;
138+
if (forward) {
139+
min_feasible_speed = current_speed + deceleration_limit_ / this->cycle_frequency_;
140+
max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_;
141+
} else {
142+
min_feasible_speed = current_speed - acceleration_limit_ / this->cycle_frequency_;
143+
max_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_;
144+
}
145+
cmd_vel->linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed);
146+
147+
// Check if we need to slow down to avoid overshooting
148+
auto remaining_distance = std::fabs(command_x_) - distance;
149+
double max_vel_to_stop = std::sqrt(-2.0 * deceleration_limit_ * remaining_distance);
150+
if (max_vel_to_stop < std::abs(cmd_vel->linear.x)) {
151+
cmd_vel->linear.x = forward ? max_vel_to_stop : -max_vel_to_stop;
152+
}
153+
}
154+
155+
// Ensure we don't go below minimum speed
156+
if (std::fabs(cmd_vel->linear.x) < minimum_speed_) {
157+
cmd_vel->linear.x = forward ? minimum_speed_ : -minimum_speed_;
158+
}
130159

131160
geometry_msgs::msg::Pose2D pose2d;
132161
pose2d.x = current_pose.pose.position.x;
@@ -139,11 +168,19 @@ class DriveOnHeading : public TimedBehavior<ActionT>
139168
return ResultStatus{Status::FAILED, ActionT::Goal::COLLISION_AHEAD};
140169
}
141170

171+
last_vel_ = cmd_vel->linear.x;
142172
this->vel_pub_->publish(std::move(cmd_vel));
143173

144174
return ResultStatus{Status::RUNNING, ActionT::Goal::NONE};
145175
}
146176

177+
void onCleanup() override {last_vel_ = std::numeric_limits<double>::max();}
178+
179+
void onActionCompletion() override
180+
{
181+
last_vel_ = std::numeric_limits<double>::max();
182+
}
183+
147184
protected:
148185
/**
149186
* @brief Check if pose is collision free
@@ -198,6 +235,26 @@ class DriveOnHeading : public TimedBehavior<ActionT>
198235
node,
199236
sim_ahead_time, rclcpp::ParameterValue(2.0));
200237
node->get_parameter(sim_ahead_time, simulate_ahead_time_);
238+
239+
nav2_util::declare_parameter_if_not_declared(
240+
node, this->behavior_name_ + ".acceleration_limit",
241+
rclcpp::ParameterValue(0.0));
242+
nav2_util::declare_parameter_if_not_declared(
243+
node, this->behavior_name_ + ".deceleration_limit",
244+
rclcpp::ParameterValue(0.0));
245+
nav2_util::declare_parameter_if_not_declared(
246+
node, this->behavior_name_ + ".minimum_speed",
247+
rclcpp::ParameterValue(0.0));
248+
node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_);
249+
node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
250+
node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_);
251+
if (acceleration_limit_ < 0.0 || deceleration_limit_ > 0.0) {
252+
RCLCPP_ERROR(this->logger_,
253+
"DriveOnHeading: acceleration_limit and deceleration_limit must be "
254+
"positive and negative respectively");
255+
acceleration_limit_ = std::abs(acceleration_limit_);
256+
deceleration_limit_ = -std::abs(deceleration_limit_);
257+
}
201258
}
202259

203260
typename ActionT::Feedback::SharedPtr feedback_;
@@ -208,6 +265,10 @@ class DriveOnHeading : public TimedBehavior<ActionT>
208265
rclcpp::Duration command_time_allowance_{0, 0};
209266
rclcpp::Time end_time_;
210267
double simulate_ahead_time_;
268+
double acceleration_limit_;
269+
double deceleration_limit_;
270+
double minimum_speed_;
271+
double last_vel_ = std::numeric_limits<double>::max();
211272
};
212273

213274
} // namespace nav2_behaviors

‎nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <memory>
2121
#include <algorithm>
2222
#include <mutex>
23+
#include <limits>
2324

2425
#include "rclcpp/rclcpp.hpp"
2526
#include "pluginlib/class_loader.hpp"
@@ -176,6 +177,8 @@ class RotationShimController : public nav2_core::Controller
176177
double rotate_to_heading_angular_vel_, max_angular_accel_;
177178
double control_duration_, simulate_ahead_time_;
178179
bool rotate_to_goal_heading_, in_rotation_;
180+
bool closed_loop_;
181+
double last_angular_vel_ = std::numeric_limits<double>::max();
179182

180183
// Dynamic parameters handler
181184
std::mutex mutex_;

‎nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp‎

Lines changed: 22 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,8 @@ void RotationShimController::configure(
6666
node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING);
6767
nav2_util::declare_parameter_if_not_declared(
6868
node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));
69+
nav2_util::declare_parameter_if_not_declared(
70+
node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true));
6971

7072
node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
7173
node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
@@ -81,6 +83,7 @@ void RotationShimController::configure(
8183
control_duration_ = 1.0 / control_frequency;
8284

8385
node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
86+
node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_);
8487

8588
try {
8689
primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
@@ -111,6 +114,7 @@ void RotationShimController::activate()
111114

112115
primary_controller_->activate();
113116
in_rotation_ = false;
117+
last_angular_vel_ = std::numeric_limits<double>::max();
114118

115119
auto node = node_.lock();
116120
dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -173,7 +177,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
173177

174178
double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw);
175179

176-
return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
180+
auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
181+
last_angular_vel_ = cmd_vel.twist.angular.z;
182+
return cmd_vel;
177183
}
178184
} catch (const std::runtime_error & e) {
179185
RCLCPP_INFO(
@@ -202,7 +208,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
202208
logger_,
203209
"Robot is not within the new path's rough heading, rotating to heading...");
204210
in_rotation_ = true;
205-
return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
211+
auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
212+
last_angular_vel_ = cmd_vel.twist.angular.z;
213+
return cmd_vel;
206214
} else {
207215
RCLCPP_DEBUG(
208216
logger_,
@@ -221,7 +229,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
221229

222230
// If at this point, use the primary controller to path track
223231
in_rotation_ = false;
224-
return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker);
232+
auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker);
233+
last_angular_vel_ = cmd_vel.twist.angular.z;
234+
return cmd_vel;
225235
}
226236

227237
geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
@@ -285,13 +295,18 @@ RotationShimController::computeRotateToHeadingCommand(
285295
const geometry_msgs::msg::PoseStamped & pose,
286296
const geometry_msgs::msg::Twist & velocity)
287297
{
298+
auto current = closed_loop_ ? velocity.angular.z : last_angular_vel_;
299+
if (current == std::numeric_limits<double>::max()) {
300+
current = 0.0;
301+
}
302+
288303
geometry_msgs::msg::TwistStamped cmd_vel;
289304
cmd_vel.header = pose.header;
290305
const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
291306
const double angular_vel = sign * rotate_to_heading_angular_vel_;
292307
const double & dt = control_duration_;
293-
const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt;
294-
const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt;
308+
const double min_feasible_angular_speed = current - max_angular_accel_ * dt;
309+
const double max_feasible_angular_speed = current + max_angular_accel_ * dt;
295310
cmd_vel.twist.angular.z =
296311
std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
297312

@@ -382,6 +397,8 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
382397
} else if (type == ParameterType::PARAMETER_BOOL) {
383398
if (name == plugin_name_ + ".rotate_to_goal_heading") {
384399
rotate_to_goal_heading_ = parameter.as_bool();
400+
} else if (name == plugin_name_ + ".closed_loop") {
401+
closed_loop_ = parameter.as_bool();
385402
}
386403
}
387404
}

‎nav2_rotation_shim_controller/test/test_shim_controller.cpp‎

Lines changed: 81 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -309,6 +309,84 @@ TEST(RotationShimControllerTest, computeVelocityTests)
309309
EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
310310
}
311311

312+
TEST(RotationShimControllerTest, openLoopRotationTests) {
313+
auto ctrl = std::make_shared<RotationShimShim>();
314+
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
315+
std::string name = "PathFollower";
316+
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
317+
auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
318+
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
319+
rclcpp_lifecycle::State state;
320+
costmap->on_configure(state);
321+
auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
322+
323+
geometry_msgs::msg::TransformStamped transform;
324+
transform.header.frame_id = "base_link";
325+
transform.child_frame_id = "odom";
326+
transform.transform.rotation.x = 0.0;
327+
transform.transform.rotation.y = 0.0;
328+
transform.transform.rotation.z = 0.0;
329+
transform.transform.rotation.w = 1.0;
330+
tf_broadcaster->sendTransform(transform);
331+
332+
// set a valid primary controller so we can do lifecycle
333+
node->declare_parameter(
334+
"PathFollower.primary_controller",
335+
std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
336+
node->declare_parameter(
337+
"controller_frequency",
338+
20.0);
339+
node->declare_parameter(
340+
"PathFollower.rotate_to_goal_heading",
341+
true);
342+
node->declare_parameter(
343+
"PathFollower.closed_loop",
344+
false);
345+
346+
auto controller = std::make_shared<RotationShimShim>();
347+
controller->configure(node, name, tf, costmap);
348+
controller->activate();
349+
350+
// Test state update and path setting
351+
nav_msgs::msg::Path path;
352+
path.header.frame_id = "base_link";
353+
path.poses.resize(4);
354+
355+
geometry_msgs::msg::PoseStamped pose;
356+
pose.header.frame_id = "base_link";
357+
geometry_msgs::msg::Twist velocity;
358+
nav2_controller::SimpleGoalChecker checker;
359+
node->declare_parameter(
360+
"checker.xy_goal_tolerance",
361+
1.0);
362+
checker.initialize(node, "checker", costmap);
363+
364+
path.header.frame_id = "base_link";
365+
path.poses[0].pose.position.x = 0.0;
366+
path.poses[0].pose.position.y = 0.0;
367+
path.poses[1].pose.position.x = 0.05;
368+
path.poses[1].pose.position.y = 0.05;
369+
path.poses[2].pose.position.x = 0.10;
370+
path.poses[2].pose.position.y = 0.10;
371+
// goal position within checker xy_goal_tolerance
372+
path.poses[3].pose.position.x = 0.20;
373+
path.poses[3].pose.position.y = 0.20;
374+
// goal heading 45 degrees to the left
375+
path.poses[3].pose.orientation.z = -0.3826834;
376+
path.poses[3].pose.orientation.w = 0.9238795;
377+
path.poses[3].header.frame_id = "base_link";
378+
379+
// Calculate first velocity command
380+
controller->setPlan(path);
381+
auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
382+
EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4);
383+
384+
// Test second velocity command with wrong odometry
385+
velocity.angular.z = 1.8;
386+
cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
387+
EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4);
388+
}
389+
312390
TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
313391
auto ctrl = std::make_shared<RotationShimShim>();
314392
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
@@ -490,7 +568,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
490568
rclcpp::Parameter("test.max_angular_accel", 7.0),
491569
rclcpp::Parameter("test.simulate_ahead_time", 7.0),
492570
rclcpp::Parameter("test.primary_controller", std::string("HI")),
493-
rclcpp::Parameter("test.rotate_to_goal_heading", true)});
571+
rclcpp::Parameter("test.rotate_to_goal_heading", true),
572+
rclcpp::Parameter("test.closed_loop", false)});
494573

495574
rclcpp::spin_until_future_complete(
496575
node->get_node_base_interface(),
@@ -502,4 +581,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
502581
EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0);
503582
EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0);
504583
EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
584+
EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false);
505585
}

0 commit comments

Comments
 (0)