-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Bound closest pose by costmap and turnaround point (#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]>
- Loading branch information
3 people
authored
Mar 21, 2022
1 parent
ee2efb6
commit 7872c73
Showing
10 changed files
with
807 additions
and
51 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,10 +1,15 @@ | ||
| # tests for regulated PP | ||
| ament_add_gtest(test_regulated_pp | ||
| test_regulated_pp.cpp | ||
| path_utils/path_utils.cpp | ||
| ) | ||
| ament_target_dependencies(test_regulated_pp | ||
| ${dependencies} | ||
| ) | ||
| target_link_libraries(test_regulated_pp | ||
| ${library_name} | ||
| ) | ||
|
|
||
| # Path utils test | ||
| ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) | ||
| ament_target_dependencies(test_path_utils nav_msgs geometry_msgs) |
88 changes: 88 additions & 0 deletions
88
nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,88 @@ | ||
| // Copyright (c) 2022 Adam Aposhian | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include <memory> | ||
|
|
||
| #include "path_utils.hpp" | ||
|
|
||
| #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" | ||
|
|
||
| namespace path_utils | ||
| { | ||
|
|
||
| void append_transform_to_path( | ||
| nav_msgs::msg::Path & path, | ||
| tf2::Transform & relative_transform) | ||
| { | ||
| // Add a new empty pose | ||
| path.poses.emplace_back(); | ||
| // Get the previous, last pose (after the emplace_back so the reference isn't invalidated) | ||
| auto & previous_pose = *(path.poses.end() - 2); | ||
| auto & new_pose = path.poses.back(); | ||
|
|
||
| // get map_transform of previous_pose | ||
| tf2::Transform map_transform; | ||
| tf2::fromMsg(previous_pose.pose, map_transform); | ||
|
|
||
| tf2::Transform full_transform; | ||
| full_transform.mult(map_transform, relative_transform); | ||
|
|
||
| tf2::toMsg(full_transform, new_pose.pose); | ||
| new_pose.header.frame_id = previous_pose.header.frame_id; | ||
| } | ||
|
|
||
| void Straight::append(nav_msgs::msg::Path & path, double spacing) const | ||
| { | ||
| auto num_points = std::floor(length_ / spacing); | ||
| path.poses.reserve(path.poses.size() + num_points); | ||
| tf2::Transform translation(tf2::Quaternion::getIdentity(), tf2::Vector3(spacing, 0.0, 0.0)); | ||
| for (size_t i = 1; i <= num_points; ++i) { | ||
| append_transform_to_path(path, translation); | ||
| } | ||
| } | ||
|
|
||
| double chord_length(double radius, double radians) | ||
| { | ||
| return 2 * radius * sin(radians / 2); | ||
| } | ||
|
|
||
| void Arc::append(nav_msgs::msg::Path & path, double spacing) const | ||
| { | ||
| double length = radius_ * std::abs(radians_); | ||
| size_t num_points = std::floor(length / spacing); | ||
| double radians_per_step = radians_ / num_points; | ||
| tf2::Transform transform( | ||
| tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), radians_per_step), | ||
| tf2::Vector3(chord_length(radius_, std::abs(radians_per_step)), 0.0, 0.0)); | ||
| path.poses.reserve(path.poses.size() + num_points); | ||
| for (size_t i = 0; i < num_points; ++i) { | ||
| append_transform_to_path(path, transform); | ||
| } | ||
| } | ||
|
|
||
| nav_msgs::msg::Path generate_path( | ||
| geometry_msgs::msg::PoseStamped start, | ||
| double spacing, | ||
| std::initializer_list<std::unique_ptr<PathSegment>> segments) | ||
| { | ||
| nav_msgs::msg::Path path; | ||
| path.header = start.header; | ||
| path.poses.push_back(start); | ||
| for (const auto & segment : segments) { | ||
| segment->append(path, spacing); | ||
| } | ||
| return path; | ||
| } | ||
|
|
||
| } // namespace path_utils |
Oops, something went wrong.