From be85e52681e046afbd692f0fd67e4267bf5af9b4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Nov 2022 22:50:36 -0700 Subject: [PATCH] refactoring RPP a bit for cleanliness on way to ROSCon (#3265) * refactor for RPP on way to ROSCon * fixing header * fixing header * fixing header * fix edge cases test samplings * linting --- .../CMakeLists.txt | 5 +- .../collision_checker.hpp | 105 +++ .../parameter_handler.hpp | 104 +++ .../path_handler.hpp | 99 +++ .../regulated_pure_pursuit_controller.hpp | 123 +--- .../regulation_functions.hpp | 138 ++++ .../src/collision_checker.cpp | 174 +++++ .../src/parameter_handler.cpp | 261 ++++++++ .../src/path_handler.cpp | 137 ++++ .../src/regulated_pure_pursuit_controller.cpp | 600 ++---------------- .../test/test_regulated_pp.cpp | 51 +- 11 files changed, 1131 insertions(+), 666 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/path_handler.cpp diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6c17481925..6f84b05382 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -35,7 +35,10 @@ set(dependencies set(library_name nav2_regulated_pure_pursuit_controller) add_library(${library_name} SHARED - src/regulated_pure_pursuit_controller.cpp) + src/regulated_pure_pursuit_controller.cpp + src/collision_checker.cpp + src/parameter_handler.cpp + src/path_handler.cpp) ament_target_dependencies(${library_name} ${dependencies} diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp new file mode 100644 index 0000000000..baccc6ca20 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::CollisionChecker + * @brief Checks for collision based on a RPP control command + */ +class CollisionChecker +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::CollisionChecker + */ + CollisionChecker( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_ros, Parameters * params); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::CollisionChecker + */ + ~CollisionChecker() = default; + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @param carrot_dist Distance to the carrot for PP + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &, + const double &); + + /** + * @brief checks for collision at projected pose + * @param x Pose of pose x + * @param y Pose of pose y + * @param theta orientation of Yaw + * @return Whether in collision + */ + bool inCollision( + const double & x, + const double & y, + const double & theta); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + +protected: + rclcpp::Logger logger_ {rclcpp::get_logger("RPPCollisionChecker")}; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr> + footprint_collision_checker_; + Parameters * params_; + std::shared_ptr> carrot_arc_pub_; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp new file mode 100644 index 0000000000..63e1d215e4 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +struct Parameters +{ + double desired_linear_vel, base_desired_linear_vel; + double lookahead_dist; + double rotate_to_heading_angular_vel; + double max_lookahead_dist; + double min_lookahead_dist; + double lookahead_time; + bool use_velocity_scaled_lookahead_dist; + double min_approach_linear_velocity; + double approach_velocity_scaling_dist; + double max_allowed_time_to_collision_up_to_carrot; + bool use_regulated_linear_velocity_scaling; + bool use_cost_regulated_linear_velocity_scaling; + double cost_scaling_dist; + double cost_scaling_gain; + double inflation_cost_scaling_factor; + double regulated_linear_scaling_min_radius; + double regulated_linear_scaling_min_speed; + bool use_rotate_to_heading; + double max_angular_accel; + double rotate_to_heading_min_angle; + bool allow_reversing; + double max_robot_pose_search_dist; + bool use_interpolation; + bool use_collision_detection; + double transform_tolerance; +}; + +/** + * @class nav2_regulated_pure_pursuit_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for RPP + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::ParameterHandler + */ + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler + */ + ~ParameterHandler() = default; + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + +protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp new file mode 100644 index 0000000000..22bc0266a6 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::PathHandler + * @brief Handles input paths to transform them to local frames required + */ +class PathHandler +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler + */ + PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint + * Points ineligible to be selected as a lookahead point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) + * @param pose pose to transform + * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} + + nav_msgs::msg::Path getPlan() {return global_plan_;} + +protected: + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; + + rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; + tf2::Duration transform_tolerance_; + std::shared_ptr tf_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7c244d9393..7b28ca9720 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -22,14 +22,15 @@ #include #include -#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/controller.hpp" #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulation_functions.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -111,28 +112,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: - /** - * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint - * Points ineligible to be selected as a lookahead point if they are any of the following: - * - Outside the local_costmap (collision avoidance cannot be assured) - * @param pose pose to transform - * @return Path in new frame - */ - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose); - - /** - * @brief Transform a pose to another frame. - * @param frame Frame ID to transform to - * @param in_pose Pose input to transform - * @param out_pose transformed output - * @return bool if successful - */ - bool transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - /** * @brief Get lookahead distance * @param cmd the current speed to use to compute lookahead point @@ -175,48 +154,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double & linear_vel, double & angular_vel, const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); - /** - * @brief Whether collision is imminent - * @param robot_pose Pose of robot - * @param carrot_pose Pose of carrot - * @param linear_vel linear velocity to forward project - * @param angular_vel angular velocity to forward project - * @param carrot_dist Distance to the carrot for PP - * @return Whether collision is imminent - */ - bool isCollisionImminent( - const geometry_msgs::msg::PoseStamped &, - const double &, const double &, - const double &); - - /** - * @brief checks for collision at projected pose - * @param x Pose of pose x - * @param y Pose of pose y - * @param theta orientation of Yaw - * @return Whether in collision - */ - bool inCollision( - const double & x, - const double & y, - const double & theta); - /** - * @brief Cost at a point - * @param x Pose of pose x - * @param y Pose of pose y - * @return Cost of pose in costmap - */ - double costAtPose(const double & x, const double & y); - - double approachVelocityScalingFactor( - const nav_msgs::msg::Path & path - ) const; - - void applyApproachVelocityScaling( - const nav_msgs::msg::Path & path, - double & linear_vel - ) const; - /** * @brief apply regulation constraints to the system * @param linear_vel robot command linear velocity input @@ -259,66 +196,24 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); - /** - * Get the greatest extent of the costmap in meters from the center. - * @return max of distance from center in meters to edge of costmap - */ - double getCostmapMaxExtent() const; - - /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; - rclcpp::Clock::SharedPtr clock_; - double desired_linear_vel_, base_desired_linear_vel_; - double lookahead_dist_; - double rotate_to_heading_angular_vel_; - double max_lookahead_dist_; - double min_lookahead_dist_; - double lookahead_time_; - bool use_velocity_scaled_lookahead_dist_; - tf2::Duration transform_tolerance_; - double min_approach_linear_velocity_; - double approach_velocity_scaling_dist_; - double control_duration_; - double max_allowed_time_to_collision_up_to_carrot_; - bool use_collision_detection_; - bool use_regulated_linear_velocity_scaling_; - bool use_cost_regulated_linear_velocity_scaling_; - double cost_scaling_dist_; - double cost_scaling_gain_; - double inflation_cost_scaling_factor_; - double regulated_linear_scaling_min_radius_; - double regulated_linear_scaling_min_speed_; - bool use_rotate_to_heading_; - double max_angular_accel_; - double rotate_to_heading_min_angle_; + Parameters * params_; double goal_dist_tol_; - bool allow_reversing_; - double max_robot_pose_search_dist_; - bool use_interpolation_; + double control_duration_; - nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - std::unique_ptr> - collision_checker_; - - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::unique_ptr path_handler_; + std::unique_ptr param_handler_; + std::unique_ptr collision_checker_; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp new file mode 100644 index 0000000000..61dca3487e --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp @@ -0,0 +1,138 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +namespace heuristics +{ + +/** + * @brief apply curvature constraint regulation on the linear velocity + * @param raw_linear_velocity Raw linear velocity desired + * @param curvature Curvature of the current command to follow the path + * @param min_radius Minimum path radius to apply the heuristic + * @return Velocity after applying the curvature constraint + */ +inline double curvatureConstraint( + const double raw_linear_vel, const double curvature, const double min_radius) +{ + const double radius = fabs(1.0 / curvature); + if (radius < min_radius) { + return raw_linear_vel * (1.0 - (fabs(radius - min_radius) / min_radius)); + } else { + return raw_linear_vel; + } +} + +/** + * @brief apply cost constraint regulation on the linear velocity + * @param raw_linear_velocity Raw linear velocity desired + * @param pose_cost Cost at the robot pose + * @param costmap_ros Costmap object to query + * @param params Parameters + * @return Velocity after applying the curvature constraint + */ +inline double costConstraint( + const double raw_linear_vel, + const double pose_cost, + std::shared_ptr costmap_ros, + Parameters * params) +{ + using namespace nav2_costmap_2d; // NOLINT + + if (pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double & inscribed_radius = costmap_ros->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / params->inflation_cost_scaling_factor) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < params->cost_scaling_dist) { + return raw_linear_vel * + (params->cost_scaling_gain * min_distance_to_obstacle / params->cost_scaling_dist); + } + } + + return raw_linear_vel; +} + +/** + * @brief Compute the scale factor to apply for linear velocity regulation on approach to goal + * @param transformed_path Path to use to calculate distances to goal + * @param approach_velocity_scaling_dist Minimum distance away to which to apply the heuristic + * @return A scale from 0.0-1.0 of the distance to goal scaled by minimum distance + */ +inline double approachVelocityScalingFactor( + const nav_msgs::msg::Path & transformed_path, + const double approach_velocity_scaling_dist) +{ + using namespace nav2_util::geometry_utils; // NOLINT + + // Waiting to apply the threshold based on integrated distance ensures we don't + // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. + const double remaining_distance = calculate_path_length(transformed_path); + if (remaining_distance < approach_velocity_scaling_dist) { + auto & last = transformed_path.poses.back(); + // Here we will use a regular euclidean distance from the robot frame (origin) + // to get smooth scaling, regardless of path density. + return std::hypot(last.pose.position.x, last.pose.position.y) / approach_velocity_scaling_dist; + } else { + return 1.0; + } +} + +/** + * @brief Velocity on approach to goal heuristic regulation term + * @param constrained_linear_vel Linear velocity already constrained by heuristics + * @param path The path plan in the robot base frame coordinates + * @param min_approach_velocity Minimum velocity to use on approach to goal + * @param approach_velocity_scaling_dist Distance away from goal to start applying this heuristic + * @return Velocity after regulation via approach to goal slow-down + */ +inline double approachVelocityConstraint( + const double constrained_linear_vel, + const nav_msgs::msg::Path & path, + const double min_approach_velocity, + const double approach_velocity_scaling_dist) +{ + double velocity_scaling = approachVelocityScalingFactor(path, approach_velocity_scaling_dist); + double approach_vel = constrained_linear_vel * velocity_scaling; + + if (approach_vel < min_approach_velocity) { + approach_vel = min_approach_velocity; + } + + return std::min(constrained_linear_vel, approach_vel); +} + +} // namespace heuristics + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp new file mode 100644 index 0000000000..ac654fcab7 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -0,0 +1,174 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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 +#include +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using namespace nav2_costmap_2d; // NOLINT + +CollisionChecker::CollisionChecker( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_ros, + Parameters * params) +{ + clock_ = node->get_clock(); + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + params_ = params; + + // initialize collision checker and set costmap + footprint_collision_checker_ = std::make_unique>(costmap_); + footprint_collision_checker_->setCostmap(costmap_); + + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); + carrot_arc_pub_->on_activate(); +} + +bool CollisionChecker::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel, + const double & carrot_dist) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. Just how the data comes to us + + // check current point is OK + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation))) + { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + double projection_time = 0.0; + if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { + // rotating to heading at goal or toward path + // Equation finds the angular distance required for the largest + // part of the robot radius to move to another costmap cell: + // theta_min = 2.0 * sin ((res/2) / r_max) + // via isosceles triangle r_max-r_max-resolution, + // dividing by angular_velocity gives us a timestep. + double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + projection_time = + 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); + } else { + // Normal path tracking + projection_time = costmap_->getResolution() / fabs(linear_vel); + } + + const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + // only forward simulate within time requested + int i = 1; + while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) { + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // check if past carrot pose, where no longer a thoughtfully valid command + if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { + break; + } + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at the projected pose + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool CollisionChecker::inCollision( + const double & x, + const double & y, + const double & theta) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } + + double footprint_cost = footprint_collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= static_cast(LETHAL_OBSTACLE); +} + + +double CollisionChecker::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_FATAL( + logger_, + "The dimensions of the costmap is too small to fully include your robot's footprint, " + "thusly the robot cannot proceed further"); + throw nav2_core::ControllerException( + "RegulatedPurePursuitController: Dimensions of the costmap are too small " + "to encapsulate the robot footprint at current speeds!"); + } + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp new file mode 100644 index 0000000000..9bb4aa9427 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -0,0 +1,261 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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 +#include +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger, + const double costmap_size_x) +{ + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_interpolation", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); + params_.base_desired_linear_vel = params_.desired_linear_vel; + node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); + node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + params_.rotate_to_heading_angular_vel); + node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", + params_.use_velocity_scaled_lookahead_dist); + node->get_parameter( + plugin_name_ + ".min_approach_linear_velocity", + params_.min_approach_linear_velocity); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + params_.approach_velocity_scaling_dist); + if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", + params_.use_regulated_linear_velocity_scaling); + node->get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + params_.use_cost_regulated_linear_velocity_scaling); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain); + node->get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", + params_.inflation_cost_scaling_factor); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", + params_.regulated_linear_scaling_min_radius); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", + params_.regulated_linear_scaling_min_speed); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); + node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", + params_.max_robot_pose_search_dist); + node->get_parameter( + plugin_name_ + ".use_interpolation", + params_.use_interpolation); + node->get_parameter( + plugin_name_ + ".use_collision_detection", + params_.use_collision_detection); + + if (params_.inflation_cost_scaling_factor <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + params_.use_cost_regulated_linear_velocity_scaling = false; + } + + /** Possible to drive in reverse direction if and only if + "use_rotate_to_heading" parameter is set to false **/ + + if (params_.use_rotate_to_heading && params_.allow_reversing) { + RCLCPP_WARN( + logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. By default setting use_rotate_to_heading true"); + params_.allow_reversing = false; + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult +ParameterHandler::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + 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_ + ".inflation_cost_scaling_factor") { + if (parameter.as_double() <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Ignoring parameter update."); + continue; + } + params_.inflation_cost_scaling_factor = parameter.as_double(); + } else if (name == plugin_name_ + ".desired_linear_vel") { + params_.desired_linear_vel = parameter.as_double(); + params_.base_desired_linear_vel = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_dist") { + params_.lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".max_lookahead_dist") { + params_.max_lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".min_lookahead_dist") { + params_.min_lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_time") { + params_.lookahead_time = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + params_.rotate_to_heading_angular_vel = parameter.as_double(); + } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + params_.min_approach_linear_velocity = parameter.as_double(); + } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_dist") { + params_.cost_scaling_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_gain") { + params_.cost_scaling_gain = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + params_.regulated_linear_scaling_min_radius = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + params_.regulated_linear_scaling_min_speed = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_accel") { + params_.max_angular_accel = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + params_.rotate_to_heading_min_angle = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_collision_detection") { + params_.use_collision_detection = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_rotate_to_heading") { + if (parameter.as_bool() && params_.allow_reversing) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + params_.use_rotate_to_heading = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_reversing") { + if (params_.use_rotate_to_heading && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + params_.allow_reversing = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp new file mode 100644 index 0000000000..f4ceec759d --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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 +#include +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using nav2_util::geometry_utils::euclidean_distance; + +PathHandler::PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros) +: transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) +{ +} + +double PathHandler::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()); + return max_costmap_dim_meters / 2.0; +} + +nav_msgs::msg::Path PathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + 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); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // We'll discard points on the plan that are outside the local costmap + const double max_costmap_extent = getCostmapMaxExtent(); + auto transformation_end = std::find_if( + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose) > max_costmap_extent; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool PathHandler::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d5c648bd1a..674a456c57 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -30,10 +30,7 @@ using std::hypot; using std::min; using std::max; using std::abs; -using nav2_util::declare_parameter_if_not_declared; -using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT -using rcl_interfaces::msg::ParameterType; namespace nav2_regulated_pure_pursuit_controller { @@ -54,157 +51,28 @@ void RegulatedPurePursuitController::configure( tf_ = tf; plugin_name_ = name; logger_ = node->get_logger(); - clock_ = node->get_clock(); - double transform_tolerance = 0.1; + // Handles storage and dynamic configuration of parameters. + // Returns pointer to data current param settings. + param_handler_ = std::make_unique( + node, plugin_name_, logger_, costmap_->getSizeInMetersX()); + params_ = param_handler_->getParams(); + + // Handles global path transformations + path_handler_ = std::make_unique( + tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_); + + // Checks for imminent collisions + collision_checker_ = std::make_unique(node, costmap_ros_, params_); + double control_frequency = 20.0; goal_dist_tol_ = 0.25; // reasonable default before first update - declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", - rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_velocity_scaling_dist", - rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_collision_detection", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(getCostmapMaxExtent())); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_interpolation", - rclcpp::ParameterValue(true)); - - node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); - base_desired_linear_vel_ = desired_linear_vel_; - node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); - node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); - node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); - node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_angular_vel", - rotate_to_heading_angular_vel_); - node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); - node->get_parameter( - plugin_name_ + ".use_velocity_scaled_lookahead_dist", - use_velocity_scaled_lookahead_dist_); - node->get_parameter( - plugin_name_ + ".min_approach_linear_velocity", - min_approach_linear_velocity_); - node->get_parameter( - plugin_name_ + ".approach_velocity_scaling_dist", - approach_velocity_scaling_dist_); - if (approach_velocity_scaling_dist_ > costmap_->getSizeInMetersX() / 2.0) { - RCLCPP_WARN( - logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " - "leading to permanent slowdown"); - } - node->get_parameter( - plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - max_allowed_time_to_collision_up_to_carrot_); - node->get_parameter( - plugin_name_ + ".use_collision_detection", - use_collision_detection_); - node->get_parameter( - plugin_name_ + ".use_regulated_linear_velocity_scaling", - use_regulated_linear_velocity_scaling_); - node->get_parameter( - plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - use_cost_regulated_linear_velocity_scaling_); - node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); - node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); - node->get_parameter( - plugin_name_ + ".inflation_cost_scaling_factor", - inflation_cost_scaling_factor_); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_radius", - regulated_linear_scaling_min_radius_); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_speed", - regulated_linear_scaling_min_speed_); - node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); - node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); - node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); - node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); node->get_parameter("controller_frequency", control_frequency); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", - max_robot_pose_search_dist_); - node->get_parameter( - plugin_name_ + ".use_interpolation", - use_interpolation_); - - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); control_duration_ = 1.0 / control_frequency; - if (inflation_cost_scaling_factor_ <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Disabling cost regulated linear velocity scaling."); - use_cost_regulated_linear_velocity_scaling_ = false; - } - - /** Possible to drive in reverse direction if and only if - "use_rotate_to_heading" parameter is set to false **/ - - if (use_rotate_to_heading_ && allow_reversing_) { - RCLCPP_WARN( - logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. By default setting use_rotate_to_heading true"); - allow_reversing_ = false; - } - global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); - carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); - - // initialize collision checker and set costmap - collision_checker_ = std::make_unique>(costmap_); - collision_checker_->setCostmap(costmap_); } void RegulatedPurePursuitController::cleanup() @@ -216,7 +84,6 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); - carrot_arc_pub_.reset(); } void RegulatedPurePursuitController::activate() @@ -228,13 +95,6 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); - carrot_arc_pub_->on_activate(); - // Add callback for dynamic parameters - auto node = node_.lock(); - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &RegulatedPurePursuitController::dynamicParametersCallback, - this, std::placeholders::_1)); } void RegulatedPurePursuitController::deactivate() @@ -246,8 +106,6 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); - carrot_arc_pub_->on_deactivate(); - dyn_params_handler_.reset(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -266,10 +124,11 @@ double RegulatedPurePursuitController::getLookAheadDistance( { // If using velocity-scaled look ahead distances, find and clamp the dist // Else, use the static look ahead distance - double lookahead_dist = lookahead_dist_; - if (use_velocity_scaled_lookahead_dist_) { - lookahead_dist = fabs(speed.linear.x) * lookahead_time_; - lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + double lookahead_dist = params_->lookahead_dist; + if (params_->use_velocity_scaled_lookahead_dist) { + lookahead_dist = fabs(speed.linear.x) * params_->lookahead_time; + lookahead_dist = std::clamp( + lookahead_dist, params_->min_lookahead_dist, params_->max_lookahead_dist); } return lookahead_dist; @@ -280,7 +139,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker) { - std::lock_guard lock_reinit(mutex_); + std::lock_guard lock_reinit(param_handler_->getMutex()); // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; @@ -292,15 +151,17 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Transform path to robot base frame - auto transformed_plan = transformGlobalPlan(pose); + auto transformed_plan = path_handler_->transformGlobalPlan( + pose, params_->max_robot_pose_search_dist); + global_path_pub_->publish(transformed_plan); // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); // Check for reverse driving - if (allow_reversing_) { + if (params_->allow_reversing) { // Cusp check - double dist_to_cusp = findVelocitySignChange(transformed_plan); + const double dist_to_cusp = findVelocitySignChange(transformed_plan); // if the lookahead distance is further than the cusp, use the cusp distance instead if (dist_to_cusp < lookahead_dist) { @@ -308,6 +169,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } } + // Get the particular point on the path at the lookahead distance auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -327,11 +189,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Setting the velocity direction double sign = 1.0; - if (allow_reversing_) { + if (params_->allow_reversing) { sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } - linear_vel = desired_linear_vel_; + linear_vel = params_->desired_linear_vel; // Make sure we're in compliance with basic constraints double angle_to_heading; @@ -343,7 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { applyConstraints( curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity @@ -352,7 +214,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + if (params_->use_collision_detection && + collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) + { throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!"); } @@ -369,7 +233,8 @@ bool RegulatedPurePursuitController::shouldRotateToPath( { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); - return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; + return params_->use_rotate_to_heading && + fabs(angle_to_path) > params_->rotate_to_heading_min_angle; } bool RegulatedPurePursuitController::shouldRotateToGoalHeading( @@ -377,7 +242,7 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( { // Whether we should rotate robot to goal heading double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; + return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( @@ -387,11 +252,11 @@ void RegulatedPurePursuitController::rotateToHeading( // Rotate in place using max angular velocity / acceleration possible linear_vel = 0.0; const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; - angular_vel = sign * rotate_to_heading_angular_vel_; + angular_vel = sign * params_->rotate_to_heading_angular_vel; const double & dt = control_duration_; - const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; - const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + const double min_feasible_angular_speed = curr_speed.angular.z - params_->max_angular_accel * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + params_->max_angular_accel * dt; angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); } @@ -443,7 +308,7 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin // If the no pose is not far enough, take the last pose if (goal_pose_it == transformed_plan.poses.end()) { goal_pose_it = std::prev(transformed_plan.poses.end()); - } else if (use_interpolation_ && goal_pose_it != transformed_plan.poses.begin()) { + } else if (params_->use_interpolation && goal_pose_it != transformed_plan.poses.begin()) { // Find the point on the line segment between the two poses // that is exactly the lookahead distance away from the robot pose (the origin) // This can be found with a closed form for the intersection of a segment and a circle @@ -463,300 +328,62 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin return *goal_pose_it; } -bool RegulatedPurePursuitController::isCollisionImminent( - const geometry_msgs::msg::PoseStamped & robot_pose, - const double & linear_vel, const double & angular_vel, - const double & carrot_dist) -{ - // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in - // odom frame and the carrot_pose is in robot base frame. - - // check current point is OK - if (inCollision( - robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation))) - { - return true; - } - - // visualization messages - nav_msgs::msg::Path arc_pts_msg; - arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); - arc_pts_msg.header.stamp = robot_pose.header.stamp; - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.frame_id = arc_pts_msg.header.frame_id; - pose_msg.header.stamp = arc_pts_msg.header.stamp; - - double projection_time = 0.0; - if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { - // rotating to heading at goal or toward path - // Equation finds the angular distance required for the largest - // part of the robot radius to move to another costmap cell: - // theta_min = 2.0 * sin ((res/2) / r_max) - // via isosceles triangle r_max-r_max-resolution, - // dividing by angular_velocity gives us a timestep. - double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); - projection_time = - 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); - } else { - // Normal path tracking - projection_time = costmap_->getResolution() / fabs(linear_vel); - } - - const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; - geometry_msgs::msg::Pose2D curr_pose; - curr_pose.x = robot_pose.pose.position.x; - curr_pose.y = robot_pose.pose.position.y; - curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); - - // only forward simulate within time requested - int i = 1; - while (i * projection_time < max_allowed_time_to_collision_up_to_carrot_) { - i++; - - // apply velocity at curr_pose over distance - curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); - curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); - curr_pose.theta += projection_time * angular_vel; - - // check if past carrot pose, where no longer a thoughtfully valid command - if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { - break; - } - - // store it for visualization - pose_msg.pose.position.x = curr_pose.x; - pose_msg.pose.position.y = curr_pose.y; - pose_msg.pose.position.z = 0.01; - arc_pts_msg.poses.push_back(pose_msg); - - // check for collision at the projected pose - if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { - carrot_arc_pub_->publish(arc_pts_msg); - return true; - } - } - - carrot_arc_pub_->publish(arc_pts_msg); - - return false; -} - -bool RegulatedPurePursuitController::inCollision( - const double & x, - const double & y, - const double & theta) -{ - unsigned int mx, my; - - if (!costmap_->worldToMap(x, y, mx, my)) { - RCLCPP_WARN_THROTTLE( - logger_, *(clock_), 30000, - "The dimensions of the costmap is too small to successfully check for " - "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " - "increase your costmap size."); - return false; - } - - double footprint_cost = collision_checker_->footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint()); - if (footprint_cost == static_cast(NO_INFORMATION) && - costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) - { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost >= static_cast(LETHAL_OBSTACLE); -} - -double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) -{ - unsigned int mx, my; - - if (!costmap_->worldToMap(x, y, mx, my)) { - RCLCPP_FATAL( - logger_, - "The dimensions of the costmap is too small to fully include your robot's footprint, " - "thusly the robot cannot proceed further"); - throw nav2_core::ControllerException( - "RegulatedPurePursuitController: Dimensions of the costmap are too small " - "to encapsulate the robot footprint at current speeds!"); - } - - unsigned char cost = costmap_->getCost(mx, my); - return static_cast(cost); -} - -double RegulatedPurePursuitController::approachVelocityScalingFactor( - const nav_msgs::msg::Path & transformed_path -) const -{ - // Waiting to apply the threshold based on integrated distance ensures we don't - // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. - double remaining_distance = nav2_util::geometry_utils::calculate_path_length(transformed_path); - if (remaining_distance < approach_velocity_scaling_dist_) { - auto & last = transformed_path.poses.back(); - // Here we will use a regular euclidean distance from the robot frame (origin) - // to get smooth scaling, regardless of path density. - double distance_to_last_pose = std::hypot(last.pose.position.x, last.pose.position.y); - return distance_to_last_pose / approach_velocity_scaling_dist_; - } else { - return 1.0; - } -} - -void RegulatedPurePursuitController::applyApproachVelocityScaling( - const nav_msgs::msg::Path & path, - double & linear_vel -) const -{ - double approach_vel = linear_vel; - double velocity_scaling = approachVelocityScalingFactor(path); - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) { - approach_vel = min_approach_linear_velocity_; - } else { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - linear_vel = std::min(linear_vel, approach_vel); -} - void RegulatedPurePursuitController::applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { - double curvature_vel = linear_vel; - double cost_vel = linear_vel; + double curvature_vel = linear_vel, cost_vel = linear_vel; // limit the linear velocity by curvature - const double radius = fabs(1.0 / curvature); - const double & min_rad = regulated_linear_scaling_min_radius_; - if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { - curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + if (params_->use_regulated_linear_velocity_scaling) { + curvature_vel = heuristics::curvatureConstraint( + linear_vel, curvature, params_->regulated_linear_scaling_min_radius); } // limit the linear velocity by proximity to obstacles - if (use_cost_regulated_linear_velocity_scaling_ && - pose_cost != static_cast(NO_INFORMATION) && - pose_cost != static_cast(FREE_SPACE)) - { - const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); - const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * - std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; - - if (min_distance_to_obstacle < cost_scaling_dist_) { - cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; - } + if (params_->use_cost_regulated_linear_velocity_scaling) { + cost_vel = heuristics::costConstraint(linear_vel, pose_cost, costmap_ros_, params_); } - // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // Use the lowest of the 2 constraints, but above the minimum translational speed linear_vel = std::min(cost_vel, curvature_vel); - linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + linear_vel = std::max(linear_vel, params_->regulated_linear_scaling_min_speed); - applyApproachVelocityScaling(path, linear_vel); + // Apply constraint to reduce speed on approach to the final goal pose + linear_vel = heuristics::approachVelocityConstraint( + linear_vel, path, params_->min_approach_linear_velocity, + params_->approach_velocity_scaling_dist); // Limit linear velocities to be valid - linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); + linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->desired_linear_vel); linear_vel = sign * linear_vel; } void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { - global_plan_ = path; + path_handler_->setPlan(path); } void RegulatedPurePursuitController::setSpeedLimit( const double & speed_limit, const bool & percentage) { + std::lock_guard lock_reinit(param_handler_->getMutex()); + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value - desired_linear_vel_ = base_desired_linear_vel_; + params_->desired_linear_vel = params_->base_desired_linear_vel; } else { if (percentage) { // Speed limit is expressed in % from maximum speed of robot - desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0; + params_->desired_linear_vel = params_->base_desired_linear_vel * speed_limit / 100.0; } else { // Speed limit is expressed in absolute value - desired_linear_vel_ = speed_limit; + params_->desired_linear_vel = speed_limit; } } } -nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) -{ - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - // let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // We'll discard points on the plan that are outside the local costmap - double max_costmap_extent = getCostmapMaxExtent(); - - 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_); - - // First find the closest pose on the path to the robot - // bounded by when the path turns around (if it does) so we don't get a pose from a later - // portion of the path - auto transformation_begin = - nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // Find points up to max_transform_dist so we only transform them. - auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose) > max_costmap_extent; - }); - - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - global_path_pub_->publish(transformed_plan); - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - double RegulatedPurePursuitController::findVelocitySignChange( const nav_msgs::msg::Path & transformed_plan) { @@ -786,119 +413,6 @@ double RegulatedPurePursuitController::findVelocitySignChange( return std::numeric_limits::max(); } - -bool RegulatedPurePursuitController::transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_->transform(in_pose, out_pose, frame, transform_tolerance_); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - -double RegulatedPurePursuitController::getCostmapMaxExtent() const -{ - const double max_costmap_dim_meters = std::max( - costmap_->getSizeInMetersX(), costmap_->getSizeInMetersY()); - return max_costmap_dim_meters / 2.0; -} - - -rcl_interfaces::msg::SetParametersResult -RegulatedPurePursuitController::dynamicParametersCallback( - std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - std::lock_guard lock_reinit(mutex_); - - 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_ + ".inflation_cost_scaling_factor") { - if (parameter.as_double() <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Ignoring parameter update."); - continue; - } - inflation_cost_scaling_factor_ = parameter.as_double(); - } else if (name == plugin_name_ + ".desired_linear_vel") { - desired_linear_vel_ = parameter.as_double(); - base_desired_linear_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_dist") { - lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead_dist") { - max_lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead_dist") { - min_lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_time") { - lookahead_time_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { - rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_approach_linear_velocity") { - min_approach_linear_velocity_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { - max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_dist") { - cost_scaling_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_gain") { - cost_scaling_gain_ = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { - regulated_linear_scaling_min_radius_ = parameter.as_double(); - } else if (name == plugin_name_ + ".transform_tolerance") { - double transform_tolerance = parameter.as_double(); - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { - regulated_linear_scaling_min_speed_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { - max_angular_accel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { - rotate_to_heading_min_angle_ = parameter.as_double(); - } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { - use_velocity_scaled_lookahead_dist_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { - use_regulated_linear_velocity_scaling_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { - use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_rotate_to_heading") { - if (parameter.as_bool() && allow_reversing_) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } - use_rotate_to_heading_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_reversing") { - if (use_rotate_to_heading_ && parameter.as_bool()) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } - allow_reversing_ = parameter.as_bool(); - } - } - } - - result.successful = true; - return result; -} - } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 51bb277fed..09892755d9 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -41,9 +41,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return global_plan_;} + nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - double getSpeed() {return desired_linear_vel_;} + double getSpeed() {return params_->desired_linear_vel;} std::unique_ptr createCarrotMsgWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -51,9 +51,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return createCarrotMsg(carrot_pose); } - void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;} - void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;} - void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;} + void setVelocityScaledLookAhead() {params_->use_velocity_scaled_lookahead_dist = true;} + void setCostRegulationScaling() {params_->use_cost_regulated_linear_velocity_scaling = true;} + void resetVelocityRegulationScaling() {params_->use_regulated_linear_velocity_scaling = false;} double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) { @@ -110,7 +110,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure nav_msgs::msg::Path transformGlobalPlanWrapper( const geometry_msgs::msg::PoseStamped & pose) { - return transformGlobalPlan(pose); + return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); } }; @@ -168,8 +168,16 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) TEST(RegulatedPurePursuitTest, findVelocitySignChange) { - auto ctrl = std::make_shared(); auto node = std::make_shared("testRPPfindVelocitySignChange"); + auto ctrl = std::make_shared(); + + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "smb"; auto time = node->get_clock()->now(); @@ -604,6 +612,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false), rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false), rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.inflation_cost_scaling_factor", 1.0), rclcpp::Parameter("test.allow_reversing", false), rclcpp::Parameter("test.use_rotate_to_heading", false)}); @@ -630,11 +639,36 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.use_velocity_scaled_lookahead_dist").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.inflation_cost_scaling_factor").as_double(), 1.0); EXPECT_EQ( node->get_parameter( "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); + + // Should fail + auto results2 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.inflation_cost_scaling_factor", -1.0)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results2); + + auto results3 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.use_rotate_to_heading", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results3); + + auto results4 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.allow_reversing", false), + rclcpp::Parameter("test.use_rotate_to_heading", true), + rclcpp::Parameter("test.allow_reversing", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results4); } class TransformGlobalPlanTest : public ::testing::Test @@ -644,8 +678,8 @@ class TransformGlobalPlanTest : public ::testing::Test { ctrl_ = std::make_shared(); node_ = std::make_shared("testRPP"); - tf_buffer_ = std::make_shared(node_->get_clock()); costmap_ = std::make_shared("fake_costmap"); + tf_buffer_ = std::make_shared(node_->get_clock()); } void configure_costmap(uint16_t width, double resolution) @@ -736,6 +770,7 @@ TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) robot_pose.pose.position.z = 0.0; // A really big costmap // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); configure_controller(5.0); setup_transforms(robot_pose.pose.position);