refactoring RPP a bit for cleanliness on way to ROSCon

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/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,103 @@
+// 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;
+  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,98 @@
+// 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 "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_