diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index e5ea991c34a..c80f310c98a 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -50,6 +50,10 @@ set(dependencies add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) ament_target_dependencies(simple_progress_checker ${dependencies}) +add_library(rotation_progress_checker SHARED plugins/rotation_progress_checker.cpp) +target_link_libraries(rotation_progress_checker simple_progress_checker) +ament_target_dependencies(rotation_progress_checker ${dependencies}) + add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp) ament_target_dependencies(simple_goal_checker ${dependencies}) @@ -79,7 +83,7 @@ target_link_libraries(${executable_name} ${library_name}) rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name} +install(TARGETS simple_progress_checker rotation_progress_checker simple_goal_checker stopped_goal_checker ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -102,6 +106,7 @@ endif() ament_export_include_directories(include) ament_export_libraries(simple_progress_checker + rotation_progress_checker simple_goal_checker stopped_goal_checker ${library_name}) diff --git a/nav2_controller/include/nav2_controller/plugins/rotation_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/rotation_progress_checker.hpp new file mode 100644 index 00000000000..2387eda03cb --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/rotation_progress_checker.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2023 Dexory +// +// 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. + +#ifndef NAV2_CONTROLLER__PLUGINS__ROTATION_PROGRESS_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__ROTATION_PROGRESS_CHECKER_HPP_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/progress_checker.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_controller +{ +/** +* @class RotationProgressChecker +* @brief This plugin is used to check the position and the angle of the robot to make sure +* that it is actually progressing or rotating towards a goal. +*/ + +class RotationProgressChecker : public SimpleProgressChecker +{ +public: + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) override; + bool check(geometry_msgs::msg::PoseStamped & current_pose) override; + +protected: + /** + * @brief Calculates robots movement from baseline pose + * @param pose Current pose of the robot + * @return true, if movement is greater than radius_, or false + */ + bool is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose); + + static double pose_angle_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); + + double required_movement_angle_; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__ROTATION_PROGRESS_CHECKER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 656d67fb300..91883d59136 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -53,6 +53,8 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker */ void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); + static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); + rclcpp::Clock::SharedPtr clock_; double radius_; diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index ad27127c03d..27928bc0bd5 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -4,6 +4,11 @@ Checks if distance between current and previous pose is above a threshold + + + Checks if distance and angle between current and previous pose is above a threshold + + Checks if current pose is within goal window for x,y and yaw diff --git a/nav2_controller/plugins/rotation_progress_checker.cpp b/nav2_controller/plugins/rotation_progress_checker.cpp new file mode 100644 index 00000000000..9e18015df32 --- /dev/null +++ b/nav2_controller/plugins/rotation_progress_checker.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2023 Dexory +// +// 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 "nav2_controller/plugins/rotation_progress_checker.hpp" +#include +#include +#include +#include +#include "angles/angles.h" +#include "nav_2d_utils/conversions.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + +void RotationProgressChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + plugin_name_ = plugin_name; + SimpleProgressChecker::initialize(parent, plugin_name); + auto node = parent.lock(); + + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5)); + node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&RotationProgressChecker::dynamicParametersCallback, this, _1)); +} + +bool RotationProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +{ + // relies on short circuit evaluation to not call is_robot_moved_enough if + // baseline_pose is not set. + geometry_msgs::msg::Pose2D current_pose2d; + current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose); + + if ((!baseline_pose_set_) || (RotationProgressChecker::is_robot_moved_enough(current_pose2d))) { + reset_baseline_pose(current_pose2d); + return true; + } + return !((clock_->now() - baseline_time_) > time_allowance_); +} + +bool RotationProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) +{ + return pose_distance(pose, baseline_pose_) > radius_ || + pose_angle_distance(pose, baseline_pose_) > required_movement_angle_; +} + +double RotationProgressChecker::pose_angle_distance( + const geometry_msgs::msg::Pose2D & pose1, + const geometry_msgs::msg::Pose2D & pose2) +{ + return abs(angles::shortest_angular_distance(pose1.theta,pose2.theta)); +} + +rcl_interfaces::msg::SetParametersResult +RotationProgressChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".required_movement_angle") { + required_movement_angle_ = parameter.as_double(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::RotationProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 0b2ec3e7396..6bc4c68ec86 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -28,8 +28,6 @@ using std::placeholders::_1; namespace nav2_controller { -static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); - void SimpleProgressChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) @@ -85,7 +83,7 @@ bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose return pose_distance(pose, baseline_pose_) > radius_; } -static double pose_distance( +double SimpleProgressChecker::pose_distance( const geometry_msgs::msg::Pose2D & pose1, const geometry_msgs::msg::Pose2D & pose2) { diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 2b496d3d58f..ab663c3224a 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -75,14 +75,7 @@ install(DIRECTORY include/ DESTINATION include/ ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) - # add_subdirectory(benchmark) -endif() + ament_export_libraries(${libraries}) ament_export_dependencies(${dependencies_pkgs}) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e1a518ae841..061654aa659 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -248,7 +248,7 @@ class Optimizer models::State state_; models::ControlSequence control_sequence_; - std::array control_history_; + std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; xt::xtensor costs_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index c66d32257e7..925d61ad636 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -88,6 +88,8 @@ class PathHandler */ nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + protected: /** * @brief Transform a pose to another frame @@ -127,19 +129,25 @@ class PathHandler * @brief Prune global path to only interesting portions * @param end Final path iterator */ - void pruneGlobalPlan(const PathIterator end); + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); std::string name_; std::shared_ptr costmap_; std::shared_ptr tf_buffer_; ParametersHandler * parameters_handler_; + nav_msgs::msg::Path global_plan_up_to_inversion_; nav_msgs::msg::Path global_plan_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; + + bool enforce_inversion_{false}; + bool inversion_remaining_{false}; + double inversion_xy_tolerance_{0}; + double inversion_yaw_tolerance{0}; }; } // namespace mppi diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 127bc318ae3..2577e82488b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -29,6 +29,7 @@ #include "angles/angles.h" +#include "nav2_mppi_controller/tools/path_handler.hpp" #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -418,13 +419,20 @@ inline void setPathCostsIfNotSet( * @param point_y Point to find angle relative to Y axis * @return Angle between two points */ -inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, + double point_yaw) { double pose_x = pose.position.x; double pose_y = pose.position.y; double pose_yaw = tf2::getYaw(pose.orientation); double yaw = atan2(point_y - pose_y, point_x - pose_x); + + if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { + yaw = angles::normalize_angle(yaw + M_PI); + } + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } @@ -436,14 +444,14 @@ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point */ inline void savitskyGolayFilter( models::ControlSequence & control_sequence, - std::array & control_history, + std::array & control_history, const models::OptimizerSettings & settings) { - // Savitzky-Golay Quadratic, 5-point Coefficients - xt::xarray filter = {-3.0, 12.0, 17.0, 12.0, -3.0}; - filter /= 35.0; + // Savitzky-Golay Quadratic, 7-point Coefficients + xt::xarray filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; + filter /= 231.0; - const unsigned int num_sequences = control_sequence.vx.shape(0); + const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; // Too short to smooth meaningfully if (num_sequences < 10) { @@ -455,69 +463,193 @@ inline void savitskyGolayFilter( }; auto applyFilterOverAxis = - [&](xt::xtensor & sequence, const float hist_0, const float hist_1) -> void + [&](xt::xtensor & sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { unsigned int idx = 0; sequence(idx) = applyFilter( { hist_0, hist_1, + hist_2, + hist_3, sequence(idx), sequence(idx + 1), - sequence(idx + 2)}); + sequence(idx + 2), + sequence(idx + 3), + sequence(idx + 4)}); idx++; sequence(idx) = applyFilter( { hist_1, + hist_2, + hist_3, sequence(idx - 1), sequence(idx), sequence(idx + 1), - sequence(idx + 2)}); + sequence(idx + 2), + sequence(idx + 3), + sequence(idx + 4)}); - for (idx = 2; idx != num_sequences - 3; idx++) { + idx++; + sequence(idx) = applyFilter( + { + hist_2, + hist_3, + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2), + sequence(idx + 3), + sequence(idx + 4)}); + + idx++; + sequence(idx) = applyFilter( + { + hist_3, + sequence(idx - 3), + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2), + sequence(idx + 3), + sequence(idx + 4)}); + + for (idx = 4; idx != num_sequences - 4; idx++) { sequence(idx) = applyFilter( { + sequence(idx - 4), + sequence(idx - 3), sequence(idx - 2), sequence(idx - 1), sequence(idx), sequence(idx + 1), - sequence(idx + 2)}); + sequence(idx + 2), + sequence(idx + 3), + sequence(idx + 4)}); } idx++; sequence(idx) = applyFilter( { + sequence(idx - 4), + sequence(idx - 3), + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2), + sequence(idx + 3), + sequence(idx + 3)}); + + idx++; + sequence(idx) = applyFilter( + { + sequence(idx - 4), + sequence(idx - 3), sequence(idx - 2), sequence(idx - 1), sequence(idx), sequence(idx + 1), + sequence(idx + 2), + sequence(idx + 2), + sequence(idx + 2)}); + + idx++; + sequence(idx) = applyFilter( + { + sequence(idx - 4), + sequence(idx - 3), + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 1), + sequence(idx + 1), sequence(idx + 1)}); idx++; sequence(idx) = applyFilter( { + sequence(idx - 4), + sequence(idx - 3), sequence(idx - 2), sequence(idx - 1), sequence(idx), sequence(idx), + sequence(idx), + sequence(idx), sequence(idx)}); }; // Filter trajectories - applyFilterOverAxis(control_sequence.vx, control_history[0].vx, control_history[1].vx); - applyFilterOverAxis(control_sequence.vy, control_history[0].vy, control_history[1].vy); - applyFilterOverAxis(control_sequence.wz, control_history[0].wz, control_history[1].wz); + applyFilterOverAxis( + control_sequence.vx, control_history[0].vx, + control_history[1].vx, control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.vy, control_history[0].vy, + control_history[1].vy, control_history[2].vy, control_history[3].vy); + applyFilterOverAxis( + control_sequence.wz, control_history[0].wz, + control_history[1].wz, control_history[2].wz, control_history[3].wz); // Update control history unsigned int offset = settings.shift_control_sequence ? 1 : 0; control_history[0] = control_history[1]; - control_history[1] = { + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = { control_sequence.vx(offset), control_sequence.vy(offset), control_sequence.wz(offset)}; } +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @path path to check for inversion + */ +inline PathIterator findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.end(); + } + + for (PathIterator it = std::next(path.poses.begin()); it != std::prev(path.poses.end()); it++) { + std::prev(it); + double a_angle = atan2( + it->pose.position.y - std::prev(it)->pose.position.y, + it->pose.position.x - std::prev(it)->pose.position.x); + double b_angle = atan2( + std::next(it)->pose.position.y - it->pose.position.y, + std::next(it)->pose.position.x - it->pose.position.x); + double angle_increment = angles::shortest_angular_distance(a_angle, b_angle); + + if (fabs(angle_increment) > M_PI_2) { + return std::next(it); + } + } + + return path.poses.end(); +} + +inline bool removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + PathIterator first_inversion = findFirstPathInversion(cropped_path); + cropped_path.poses.erase(first_inversion, cropped_path.poses.end()); + + if (path.poses.size() == cropped_path.poses.size()) { + return false; + } else { + path = cropped_path; + return true; + } +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a588bbe2913..ce94153d5f4 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -57,16 +57,30 @@ void PathAngleCritic::score(CriticData & data) const float goal_x = xt::view(data.path.x, offseted_idx); const float goal_y = xt::view(data.path.y, offseted_idx); + const float goal_yaw = xt::view(data.path.yaws, offseted_idx); - if (utils::posePointAngle(data.state.pose.pose, goal_x, goal_y) < max_angle_to_furthest_) { + if (utils::posePointAngle( + data.state.pose.pose, goal_x, goal_y, + goal_yaw) < max_angle_to_furthest_) + { return; } const auto yaws_between_points = xt::atan2( goal_y - data.trajectories.y, goal_x - data.trajectories.x); + + const auto yaws_between_points_corrected = xt::eval( + xt::where( + xt::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) <= M_PI_2, + yaws_between_points, + utils::normalize_angles(yaws_between_points + M_PI))); + const auto yaws = - xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); + xt::abs( + utils::shortest_angular_distance( + data.trajectories.yaws, + yaws_between_points_corrected)); data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 4adf5552206..55362fd7a8a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -119,6 +119,8 @@ void Optimizer::reset() control_sequence_.reset(settings_.time_steps); control_history_[0] = {0.0, 0.0, 0.0}; control_history_[1] = {0.0, 0.0, 0.0}; + control_history_[2] = {0.0, 0.0, 0.0}; + control_history_[3] = {0.0, 0.0, 0.0}; costs_ = xt::zeros({settings_.batch_size}); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 83ec415e172..7eaaca60998 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -35,6 +35,11 @@ void PathHandler::initialize( getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); getParam(prune_distance_, "prune_distance", 1.5); getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_inversion_, "enforce_inversion", false); + if (enforce_inversion_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.1); + getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.1); + } } std::pair @@ -43,12 +48,13 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( { using nav2_util::geometry_utils::euclidean_distance; - auto begin = global_plan_.poses.begin(); - auto end = global_plan_.poses.end(); + auto begin = global_plan_up_to_inversion_.poses.begin(); + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( @@ -61,17 +67,17 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); transformed_plan.header.stamp = global_pose.header.stamp; + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + unsigned int mx, my; // Find the furthest relevent pose on the path to consider within costmap // bounds // Transforming it to the costmap frame in the same loop - for (auto global_plan_pose = closest_point; global_plan_pose != end; ++global_plan_pose) { - // Distance relative to robot pose check - auto distance = euclidean_distance(global_pose, *global_plan_pose); - if (distance >= prune_distance_) { - return {transformed_plan, closest_point}; - } - + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { // Transform from global plan frame to costmap frame geometry_msgs::msg::PoseStamped costmap_plan_pose; global_plan_pose->header.stamp = global_pose.header.stamp; @@ -94,12 +100,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { throw nav2_core::ControllerTFError( "Unable to transform robot pose into global plan's frame"); } @@ -113,9 +119,19 @@ nav_msgs::msg::Path PathHandler::transformPath( // Find relevent bounds of path to use geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(robot_pose); + + auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - pruneGlobalPlan(lower_bound); + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if (enforce_inversion_ && inversion_remaining_) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, utils::findFirstPathInversion(global_plan_)); + global_plan_up_to_inversion_ = global_plan_; + inversion_remaining_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); @@ -155,13 +171,31 @@ double PathHandler::getMaxCostmapDist() void PathHandler::setPath(const nav_msgs::msg::Path & plan) { global_plan_ = plan; + global_plan_up_to_inversion_ = global_plan_; + if (enforce_inversion_) { + inversion_remaining_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } } nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} -void PathHandler::pruneGlobalPlan(const PathIterator end) +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) { - global_plan_.poses.erase(global_plan_.poses.begin(), end); + // Keep full path if we are within tolerance of the inversion pose + double distance = std::hypot( + robot_pose.pose.position.x - global_plan_up_to_inversion_.poses.end()->pose.position.x, + robot_pose.pose.position.y - global_plan_up_to_inversion_.poses.end()->pose.position.y); + + double angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(global_plan_up_to_inversion_.poses.end()->pose.orientation)); + + return distance < inversion_xy_tolerance_ && fabs(angle_distance) < inversion_yaw_tolerance; } } // namespace mppi diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 2e1179dd5b6..4a299b2fd8e 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -38,9 +38,9 @@ class PathHandlerWrapper : public PathHandler PathHandlerWrapper() : PathHandler() {} - void pruneGlobalPlanWrapper(const PathIterator end) + void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) { - return pruneGlobalPlan(end); + return prunePlan(path, end); } double getMaxCostmapDistWrapper() @@ -82,7 +82,7 @@ TEST(PathHandlerTests, GetAndPrunePath) EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); PathIterator it = rtn_path.poses.begin() + 5; - handler.pruneGlobalPlanWrapper(it); + handler.pruneGlobalPlanWrapper(path, it); auto rtn2_path = handler.getPath(); EXPECT_EQ(rtn2_path.poses.size(), 6u); } @@ -109,7 +109,6 @@ TEST(PathHandlerTests, TestBounds) std::unique_ptr tf_broadcaster_ = std::make_unique(node); geometry_msgs::msg::TransformStamped t; - //t.header.stamp = node->get_clock()->now(); t.header.frame_id = "map"; t.child_frame_id = "base_link"; tf_broadcaster_->sendTransform(t); @@ -134,7 +133,7 @@ TEST(PathHandlerTests, TestBounds) handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); auto & path_in = handler.getPath(); EXPECT_EQ(closest - path_in.poses.begin(), 25); - handler.pruneGlobalPlanWrapper(closest); + handler.pruneGlobalPlanWrapper(path, closest); auto & path_pruned = handler.getPath(); EXPECT_EQ(path_pruned.poses.size(), 75u); } @@ -157,7 +156,6 @@ TEST(PathHandlerTests, TestTransforms) std::unique_ptr tf_broadcaster_ = std::make_unique(node); geometry_msgs::msg::TransformStamped t; - //t.header.stamp = node->get_clock()->now(); t.header.frame_id = "map"; t.child_frame_id = "base_link"; tf_broadcaster_->sendTransform(t);