Skip to content

Commit 7872c73

Browse files
Aposhiansathak93
Aposhian
and
sathak93
authored
Bound closest pose by costmap and turnaround point (ros-navigation#2812)
* created integrated distance util * chng transform begin , end and always limit lp pt * chg trnsfm begin end and use euclidean distance * linting fix * linting fix * limit trnsfm to cusp * remove print * linting fix * lint fix * hypot * use direction change as upper bound for closest pose * bound closest waypoint by direction change * use max_transform_dist for closest_pose_upper_bound * remove first_element_beyond optimization todo * added path_utils for test path generation * initial test for path_utils * added full failing test for path_utils * fixed path_utils segfault * properly initialize straight transform * fixed right turn infinite loop * fixed path_utils transforms for half-turns * all path_utils tests passing * added half turn test for path_utils * remove unused dependencies from test_path_utils * move path_utils classes into path_building_blocks namespace * use hypot for euclidean_distance * rename findDirectionChange to findCusp * use integrated distance for lookahead distance * Revert "use integrated distance for lookahead distance" This reverts commit 6e4c6cd. * parameterize transformation_begin bound * change default to a regular constant * use std::hypot for x, y, z * initial failing test for transformGlobalPlan * refactor transform global plan into separate test fixture * make TransformGlobalPlan test fixture, and fix transforms * no_pruning_on_large_costmap test passing * added another transformGlobalPlan test * added a test for all poses outside of costmap * added good shortcut test for transformGlobalPlan * added more tests for rpp costmap pruning * cpplint * remove unused rclcpp::init and rclcpp::shutdown * change default max_distance_between_iterations to max_costmap_extent * rename max_distance_between_iterations to max_robot_pose_search_dist * increase default dwb prune distance * add initial docs for max_robot_pose_search_dist to README * add note about when to set max_robot_pose_search_dist * rename first_element_beyond to first_after_integrated_distance * renamed findCusp to findVelocitySignChange * move path_utils to RPP tests * only check velocity sign change when reversing is enabled * do not check cusp for dwb transformed plan end Co-authored-by: sathak93 <[email protected]> Co-authored-by: Adam Aposhian <[email protected]>
1 parent ee2efb6 commit 7872c73

10 files changed

Lines changed: 807 additions & 51 deletions

File tree

‎nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp‎

Lines changed: 24 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -46,13 +46,15 @@
4646
#include "nav_2d_utils/conversions.hpp"
4747
#include "nav_2d_utils/parameters.hpp"
4848
#include "nav_2d_utils/tf_help.hpp"
49+
#include "nav2_util/geometry_utils.hpp"
4950
#include "nav2_util/lifecycle_node.hpp"
5051
#include "nav2_util/node_utils.hpp"
5152
#include "pluginlib/class_list_macros.hpp"
5253
#include "nav_msgs/msg/path.hpp"
5354
#include "geometry_msgs/msg/twist_stamped.hpp"
5455

5556
using nav2_util::declare_parameter_if_not_declared;
57+
using nav2_util::geometry_utils::euclidean_distance;
5658

5759
namespace dwb_core
5860
{
@@ -87,7 +89,7 @@ void DWBLocalPlanner::configure(
8789
rclcpp::ParameterValue(true));
8890
declare_parameter_if_not_declared(
8991
node, dwb_plugin_name_ + ".prune_distance",
90-
rclcpp::ParameterValue(1.0));
92+
rclcpp::ParameterValue(2.0));
9193
declare_parameter_if_not_declared(
9294
node, dwb_plugin_name_ + ".debug_trajectory_details",
9395
rclcpp::ParameterValue(false));
@@ -434,17 +436,6 @@ DWBLocalPlanner::scoreTrajectory(
434436
return score;
435437
}
436438

437-
double
438-
getSquareDistance(
439-
const geometry_msgs::msg::Pose2D & pose_a,
440-
const geometry_msgs::msg::Pose2D & pose_b)
441-
{
442-
double x_diff = pose_a.x - pose_b.x;
443-
double y_diff = pose_a.y - pose_b.y;
444-
445-
return x_diff * x_diff + y_diff * y_diff;
446-
}
447-
448439
nav_2d_msgs::msg::Path2D
449440
DWBLocalPlanner::transformGlobalPlan(
450441
const nav_2d_msgs::msg::Pose2DStamped & pose)
@@ -467,46 +458,51 @@ DWBLocalPlanner::transformGlobalPlan(
467458
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
468459
double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) *
469460
costmap->getResolution() / 2.0;
470-
double sq_dist_threshold = dist_threshold * dist_threshold;
461+
471462

472463
// If prune_plan is enabled (it is by default) then we want to restrict the
473464
// plan to distances within that range as well.
474-
double sq_prune_dist = prune_distance_ * prune_distance_;
465+
double prune_dist = prune_distance_;
475466

476467
// Set the maximum distance we'll include points before getting to the part
477468
// of the path where the robot is located (the start of the plan). Basically,
478469
// these are the points the robot has already passed.
479-
double sq_transform_start_threshold;
470+
double transform_start_threshold;
480471
if (prune_plan_) {
481-
sq_transform_start_threshold = std::min(sq_dist_threshold, sq_prune_dist);
472+
transform_start_threshold = std::min(dist_threshold, prune_dist);
482473
} else {
483-
sq_transform_start_threshold = sq_dist_threshold;
474+
transform_start_threshold = dist_threshold;
484475
}
485476

486477
// Set the maximum distance we'll include points after the part of the part of
487478
// the plan near the robot (the end of the plan). This determines the amount
488479
// of the plan passed on to the critics
489-
double sq_transform_end_threshold;
480+
double transform_end_threshold;
490481
if (shorten_transformed_plan_) {
491-
sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist);
482+
transform_end_threshold = std::min(dist_threshold, prune_dist);
492483
} else {
493-
sq_transform_end_threshold = sq_dist_threshold;
484+
transform_end_threshold = dist_threshold;
494485
}
495486

496-
// Find the first pose in the plan that's less than sq_transform_start_threshold
487+
// Find the first pose in the global plan that's further than prune distance
488+
// from the robot using integrated distance
489+
auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance(
490+
global_plan_.poses.begin(), global_plan_.poses.end(), prune_dist);
491+
492+
// Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold
497493
// from the robot.
498494
auto transformation_begin = std::find_if(
499-
begin(global_plan_.poses), end(global_plan_.poses),
495+
begin(global_plan_.poses), prune_point,
500496
[&](const auto & global_plan_pose) {
501-
return getSquareDistance(robot_pose.pose, global_plan_pose) < sq_transform_start_threshold;
497+
return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold;
502498
});
503499

504-
// Find the first pose in the end of the plan that's further than sq_transform_end_threshold
505-
// from the robot
500+
// Find the first pose in the end of the plan that's further than transform_end_threshold
501+
// from the robot using integrated distance
506502
auto transformation_end = std::find_if(
507-
transformation_begin, end(global_plan_.poses),
508-
[&](const auto & global_plan_pose) {
509-
return getSquareDistance(robot_pose.pose, global_plan_pose) > sq_transform_end_threshold;
503+
transformation_begin, global_plan_.poses.end(),
504+
[&](const auto & pose) {
505+
return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold;
510506
});
511507

512508
// Transform the near part of the global plan into the robot's frame of reference.

‎nav2_regulated_pure_pursuit_controller/README.md‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
7575
| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. |
7676
| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. |
7777
| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled |
78+
| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. |
7879

7980
Example fully-described XML with default parameter values:
8081

@@ -121,6 +122,7 @@ controller_server:
121122
use_rotate_to_heading: true
122123
rotate_to_heading_min_angle: 0.785
123124
max_angular_accel: 3.2
125+
max_robot_pose_search_dist: 10.0
124126
cost_scaling_dist: 0.3
125127
cost_scaling_gain: 1.0
126128
inflation_cost_scaling_factor: 3.0

‎nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -112,11 +112,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller
112112

113113
protected:
114114
/**
115-
* @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
115+
* @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint
116+
* Points ineligible to be selected as a lookahead point if they are any of the following:
117+
* - Outside the local_costmap (collision avoidance cannot be assured)
116118
* @param pose pose to transform
117119
* @return Path in new frame
118120
*/
119-
nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);
121+
nav_msgs::msg::Path transformGlobalPlan(
122+
const geometry_msgs::msg::PoseStamped & pose);
120123

121124
/**
122125
* @brief Transform a pose to another frame.
@@ -232,7 +235,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller
232235
* @param pose Pose input to determine the cusp position
233236
* @return robot distance from the cusp
234237
*/
235-
double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose);
238+
double findVelocitySignChange(const geometry_msgs::msg::PoseStamped & pose);
239+
240+
/**
241+
* Get the greatest extent of the costmap in meters from the center.
242+
* @return max of distance from center in meters to edge of costmap
243+
*/
244+
double getCostmapMaxExtent() const;
236245

237246
/**
238247
* @brief Callback executed when a parameter change is detected
@@ -272,6 +281,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
272281
double rotate_to_heading_min_angle_;
273282
double goal_dist_tol_;
274283
bool allow_reversing_;
284+
double max_robot_pose_search_dist_;
275285

276286
nav_msgs::msg::Path global_plan_;
277287
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;

‎nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp‎

Lines changed: 31 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,9 @@ void RegulatedPurePursuitController::configure(
105105
node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
106106
declare_parameter_if_not_declared(
107107
node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
108+
declare_parameter_if_not_declared(
109+
node, plugin_name_ + ".max_robot_pose_search_dist",
110+
rclcpp::ParameterValue(getCostmapMaxExtent()));
108111

109112
node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
110113
base_desired_linear_vel_ = desired_linear_vel_;
@@ -147,6 +150,9 @@ void RegulatedPurePursuitController::configure(
147150
node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
148151
node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_);
149152
node->get_parameter("controller_frequency", control_frequency);
153+
node->get_parameter(
154+
plugin_name_ + ".max_robot_pose_search_dist",
155+
max_robot_pose_search_dist_);
150156

151157
transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
152158
control_duration_ = 1.0 / control_frequency;
@@ -232,7 +238,8 @@ std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController
232238
return carrot_msg;
233239
}
234240

235-
double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed)
241+
double RegulatedPurePursuitController::getLookAheadDistance(
242+
const geometry_msgs::msg::Twist & speed)
236243
{
237244
// If using velocity-scaled look ahead distances, find and clamp the dist
238245
// Else, use the static look ahead distance
@@ -270,11 +277,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
270277
// Check for reverse driving
271278
if (allow_reversing_) {
272279
// Cusp check
273-
double dist_to_direction_change = findDirectionChange(pose);
280+
double dist_to_cusp = findVelocitySignChange(pose);
274281

275282
// if the lookahead distance is further than the cusp, use the cusp distance instead
276-
if (dist_to_direction_change < lookahead_dist) {
277-
lookahead_dist = dist_to_direction_change;
283+
if (dist_to_cusp < lookahead_dist) {
284+
lookahead_dist = dist_to_cusp;
278285
}
279286
}
280287

@@ -602,23 +609,27 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
602609
}
603610

604611
// We'll discard points on the plan that are outside the local costmap
605-
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
606-
const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
607-
const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0;
612+
double max_costmap_extent = getCostmapMaxExtent();
613+
614+
auto closest_pose_upper_bound =
615+
nav2_util::geometry_utils::first_after_integrated_distance(
616+
global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_);
608617

609618
// First find the closest pose on the path to the robot
619+
// bounded by when the path turns around (if it does) so we don't get a pose from a later
620+
// portion of the path
610621
auto transformation_begin =
611622
nav2_util::geometry_utils::min_by(
612-
global_plan_.poses.begin(), global_plan_.poses.end(),
623+
global_plan_.poses.begin(), closest_pose_upper_bound,
613624
[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
614625
return euclidean_distance(robot_pose, ps);
615626
});
616627

617-
// Find points definitely outside of the costmap so we won't transform them.
628+
// Find points up to max_transform_dist so we only transform them.
618629
auto transformation_end = std::find_if(
619-
transformation_begin, end(global_plan_.poses),
620-
[&](const auto & global_plan_pose) {
621-
return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist;
630+
transformation_begin, global_plan_.poses.end(),
631+
[&](const auto & pose) {
632+
return euclidean_distance(pose, robot_pose) > max_costmap_extent;
622633
});
623634

624635
// Lambda to transform a PoseStamped from global frame to local
@@ -652,7 +663,7 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
652663
return transformed_plan;
653664
}
654665

655-
double RegulatedPurePursuitController::findDirectionChange(
666+
double RegulatedPurePursuitController::findVelocitySignChange(
656667
const geometry_msgs::msg::PoseStamped & pose)
657668
{
658669
// Iterating through the global path to determine the position of the cusp
@@ -700,6 +711,13 @@ bool RegulatedPurePursuitController::transformPose(
700711
return false;
701712
}
702713

714+
double RegulatedPurePursuitController::getCostmapMaxExtent() const
715+
{
716+
const double max_costmap_dim_meters = std::max(
717+
costmap_->getSizeInMetersX(), costmap_->getSizeInMetersX());
718+
return max_costmap_dim_meters / 2.0;
719+
}
720+
703721

704722
rcl_interfaces::msg::SetParametersResult
705723
RegulatedPurePursuitController::dynamicParametersCallback(

‎nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,15 @@
11
# tests for regulated PP
22
ament_add_gtest(test_regulated_pp
33
test_regulated_pp.cpp
4+
path_utils/path_utils.cpp
45
)
56
ament_target_dependencies(test_regulated_pp
67
${dependencies}
78
)
89
target_link_libraries(test_regulated_pp
910
${library_name}
1011
)
12+
13+
# Path utils test
14+
ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp)
15+
ament_target_dependencies(test_path_utils nav_msgs geometry_msgs)

‎nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// Copyright (c) 2022 Adam Aposhian
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <memory>
16+
17+
#include "path_utils.hpp"
18+
19+
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
20+
21+
namespace path_utils
22+
{
23+
24+
void append_transform_to_path(
25+
nav_msgs::msg::Path & path,
26+
tf2::Transform & relative_transform)
27+
{
28+
// Add a new empty pose
29+
path.poses.emplace_back();
30+
// Get the previous, last pose (after the emplace_back so the reference isn't invalidated)
31+
auto & previous_pose = *(path.poses.end() - 2);
32+
auto & new_pose = path.poses.back();
33+
34+
// get map_transform of previous_pose
35+
tf2::Transform map_transform;
36+
tf2::fromMsg(previous_pose.pose, map_transform);
37+
38+
tf2::Transform full_transform;
39+
full_transform.mult(map_transform, relative_transform);
40+
41+
tf2::toMsg(full_transform, new_pose.pose);
42+
new_pose.header.frame_id = previous_pose.header.frame_id;
43+
}
44+
45+
void Straight::append(nav_msgs::msg::Path & path, double spacing) const
46+
{
47+
auto num_points = std::floor(length_ / spacing);
48+
path.poses.reserve(path.poses.size() + num_points);
49+
tf2::Transform translation(tf2::Quaternion::getIdentity(), tf2::Vector3(spacing, 0.0, 0.0));
50+
for (size_t i = 1; i <= num_points; ++i) {
51+
append_transform_to_path(path, translation);
52+
}
53+
}
54+
55+
double chord_length(double radius, double radians)
56+
{
57+
return 2 * radius * sin(radians / 2);
58+
}
59+
60+
void Arc::append(nav_msgs::msg::Path & path, double spacing) const
61+
{
62+
double length = radius_ * std::abs(radians_);
63+
size_t num_points = std::floor(length / spacing);
64+
double radians_per_step = radians_ / num_points;
65+
tf2::Transform transform(
66+
tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), radians_per_step),
67+
tf2::Vector3(chord_length(radius_, std::abs(radians_per_step)), 0.0, 0.0));
68+
path.poses.reserve(path.poses.size() + num_points);
69+
for (size_t i = 0; i < num_points; ++i) {
70+
append_transform_to_path(path, transform);
71+
}
72+
}
73+
74+
nav_msgs::msg::Path generate_path(
75+
geometry_msgs::msg::PoseStamped start,
76+
double spacing,
77+
std::initializer_list<std::unique_ptr<PathSegment>> segments)
78+
{
79+
nav_msgs::msg::Path path;
80+
path.header = start.header;
81+
path.poses.push_back(start);
82+
for (const auto & segment : segments) {
83+
segment->append(path, spacing);
84+
}
85+
return path;
86+
}
87+
88+
} // namespace path_utils

0 commit comments

Comments
 (0)