Merge pull request ros-navigation#4 from botsandus/AUTO-572_update_mppic

AUTO-572 update mppic

Author: doisyg

nav2_mppi_controller/README.md

@@ -245,6 +245,8 @@ Visualization of the trajectories using `visualize` uses compute resources to ba
 
 The most common parameters you might want to start off changing are the velocity profiles (`vx_max`, `vx_min`, `wz_max`, and `vy_max` if holonomic) and the `motion_model` to correspond to your vehicle. Its wise to consider the `prune_distance` of the path plan in proportion to your maximum velocity and prediction horizon. The only deeper parameter that will likely need to be adjusted for your particular settings is the Obstacle critics' `repulsion_weight` since the tuning of this is proprtional to your inflation layer's radius. Higher radii should correspond to reduced `repulsion_weight` due to the penalty formation (e.g. `inflation_radius - min_dist_to_obstacle`). If this penalty is too high, the robot will slow significantly when entering cost-space from non-cost space or jitter in narrow corridors. It is noteworthy, but likely not necessary to be changed, that the Obstacle critic may use the full footprint information if `consider_footprint = true`, though comes at an increased compute cost.
 
+If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. 
+
 ### Prediction Horizon, Costmap Sizing, and Offsets
 
 As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artifically limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width.
@@ -255,7 +257,7 @@ The Path Follow critic cannot drive velocities greater than the projectable dist
 
 ### Obstacle, Inflation Layer, and Path Following
 
-There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. It may also perform awkward maneuvors when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. 
+There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvors when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. 
 
 Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered.
 

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -25,6 +25,7 @@
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_core/goal_checker.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -27,6 +27,7 @@
 #include "builtin_interfaces/msg/time.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 #include "nav2_mppi_controller/tools/parameters_handler.hpp"
 

nav2_mppi_controller/src/optimizer.cpp

@@ -108,7 +108,7 @@ void Optimizer::setOffset(double controller_frequency)
       "shifting is ON");
     settings_.shift_control_sequence = true;
   } else {
-    throw std::runtime_error(
+    throw nav2_core::ControllerException(
             "Controller period more then model dt, set it equal to model dt");
   }
 }
@@ -170,7 +170,7 @@ bool Optimizer::fallback(bool fail)
 
   if (++counter > settings_.retry_attempt_limit) {
     counter = 0;
-    throw std::runtime_error("Optimizer fail to compute path");
+    throw nav2_core::NoValidControl("Optimizer fail to compute path");
   }
 
   return true;
@@ -407,7 +407,7 @@ void Optimizer::setMotionModel(const std::string & model)
   } else if (model == "Ackermann") {
     motion_model_ = std::make_shared<AckermannMotionModel>(parameters_handler_);
   } else {
-    throw std::runtime_error(
+    throw nav2_core::ControllerException(
             std::string(
               "Model " + model + " is not valid! Valid options are DiffDrive, Omni, "
               "or Ackermann"));

nav2_mppi_controller/src/path_handler.cpp

@@ -74,12 +74,12 @@ geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
   const geometry_msgs::msg::PoseStamped & pose)
 {
   if (global_plan_.poses.empty()) {
-    throw std::runtime_error("Received plan with zero length");
+    throw nav2_core::InvalidPath("Received plan with zero length");
   }
 
   geometry_msgs::msg::PoseStamped robot_pose;
   if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
-    throw std::runtime_error(
+    throw nav2_core::ControllerTFError(
             "Unable to transform robot pose into global plan's frame");
   }
 
@@ -102,7 +102,7 @@ nav_msgs::msg::Path PathHandler::transformPath(
   pruneGlobalPlan(lower_bound);
 
   if (transformed_plan.poses.empty()) {
-    throw std::runtime_error("Resulting plan has 0 poses in it.");
+    throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
   }
 
   return transformed_plan;

nav2_mppi_controller/test/utils/factory.hpp

@@ -56,7 +56,7 @@ void setUpOptimizerParams(
   const std::vector<std::string> & critics,
   std::vector<rclcpp::Parameter> & params_, std::string node_name = std::string("dummy"))
 {
-  constexpr double dummy_freq = 10.0;
+  constexpr double dummy_freq = 50.0;
   params_.emplace_back(rclcpp::Parameter(node_name + ".iteration_count", s.iteration_count));
   params_.emplace_back(rclcpp::Parameter(node_name + ".batch_size", s.batch_size));
   params_.emplace_back(rclcpp::Parameter(node_name + ".time_steps", s.time_steps));
@@ -70,7 +70,7 @@ void setUpControllerParams(
   bool visualize, std::vector<rclcpp::Parameter> & params_,
   std::string node_name = std::string("dummy"))
 {
-  double dummy_freq = 10.0;
+  double dummy_freq = 50.0;
   params_.emplace_back(rclcpp::Parameter(node_name + ".visualize", visualize));
   params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
 }

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_