refactoring RPP a bit for cleanliness on way to ROSCon (ros-navigatio…

…n#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

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}

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 <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <mutex>
+
+#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<nav2_costmap_2d::Costmap2DROS> 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<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+ nav2_costmap_2d::Costmap2D * costmap_;
+ std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
+ footprint_collision_checker_;
+ Parameters * params_;
+ std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
+ rclcpp::Clock::SharedPtr clock_;
+};
+
+} // namespace nav2_regulated_pure_pursuit_controller
+
+#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_

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 <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <mutex>
+
+#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 &params_;}
+
+protected:
+ /**
+ * @brief Callback executed when a parameter change is detected
+ * @param event ParameterEvent message
+ */
+ rcl_interfaces::msg::SetParametersResult
+ dynamicParametersCallback(std::vector<rclcpp::Parameter> 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_

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 <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <mutex>
+
+#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<tf2_ros::Buffer> tf,
+ std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<tf2_ros::Buffer> tf_;
+ std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+ nav_msgs::msg::Path global_plan_;
+};
+
+} // namespace nav2_regulated_pure_pursuit_controller
+
+#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_