Get custom changes from galactic

teb_local_planner/include/teb_local_planner/homotopy_class_planner.h

@@ -223,10 +223,10 @@ class HomotopyClassPlanner : public PlannerInterface
    * @param inscribed_radius The radius of the inscribed circle of the robot
    * @param circumscribed_radius The radius of the circumscribed circle of the robot
    * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
-   * @return \c true, if the robot footprint along the first part of the trajectory intersects with
-   *         any obstacle in the costmap, \c false otherwise.
+   * @return \c int >= 0, if the robot footprint along the first part of the trajectory intersects with
+   *         any obstacle in the costmap (return the index of the pose that intersects), \c -1 otherwise.
    */
-  virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+  virtual int isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
                                     double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0);
 
   /**

teb_local_planner/include/teb_local_planner/optimal_planner.h

@@ -518,10 +518,10 @@ class TebOptimalPlanner : public PlannerInterface
    * @param inscribed_radius The radius of the inscribed circle of the robot
    * @param circumscribed_radius The radius of the circumscribed circle of the robot
    * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
-   * @return \c true, if the robot footprint along the first part of the trajectory intersects with 
-   *         any obstacle in the costmap, \c false otherwise.
+   * @return int >= 0, if the robot footprint along the first part of the trajectory intersects with
+   *         any obstacle in the costmap (return the index of the pose that intersects), \c -1 otherwise.
    */
-  virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec, double inscribed_radius = 0.0,
+  virtual int isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec, double inscribed_radius = 0.0,
           double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1);
 
   /**

teb_local_planner/include/teb_local_planner/planner_interface.h

@@ -191,7 +191,7 @@ class PlannerInterface
    * @return \c true, if the robot footprint along the first part of the trajectory intersects with 
    *         any obstacle in the costmap, \c false otherwise.
    */
-  virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+  virtual int isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
         double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0) = 0;
 
   /**

teb_local_planner/include/teb_local_planner/teb_config.h

@@ -82,6 +82,8 @@ class TebConfig
     double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
     double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (skip hot-starting)
     int feasibility_check_no_poses; //!< Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.
+    int feasibility_check_stop_poses; // How many poses to use for linear slowdown / stop for feasibility check. TEB will slowdown until this pose, and stop for all indices lower than that.
+    bool feasibility_check; //!< Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval.
     double feasibility_check_lookahead_distance; //!< Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.
     bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
     double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad]
@@ -260,6 +262,8 @@ class TebConfig
     trajectory.force_reinit_new_goal_angular = 0.5 * M_PI;
     trajectory.feasibility_check_no_poses = 5;
     trajectory.feasibility_check_lookahead_distance = -1;
+    trajectory.feasibility_check_stop_poses = 2;
+    trajectory.feasibility_check = false;
     trajectory.publish_feedback = false;
     trajectory.min_resolution_collision_check_angular = M_PI;
     trajectory.control_look_ahead_poses = 1;
@@ -271,9 +275,9 @@ class TebConfig
     robot.max_vel_y = 0.0;
     robot.max_vel_theta = 0.3;
     robot.base_max_vel_x = robot.max_vel_x;
-    robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards;
-    robot.base_max_vel_y = robot.base_max_vel_y;
-    robot.base_max_vel_theta = robot.base_max_vel_theta;
+    robot.base_max_vel_x_backwards = robot.max_vel_x_backwards;
+    robot.base_max_vel_y = robot.max_vel_y;
+    robot.base_max_vel_theta = robot.max_vel_theta;
     robot.acc_lim_x = 0.5;
     robot.acc_lim_y = 0.5;
     robot.acc_lim_theta = 0.5;

teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h

@@ -239,7 +239,7 @@ class TebLocalPlannerROS : public nav2_core::Controller
     * @param obst_msg pointer to the message containing a list of polygon shaped obstacles
     */
   void customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg);
-  
+
    /**
     * @brief Callback for custom via-points
     * @param via_points_msg pointer to the message containing a list of via-points

teb_local_planner/src/homotopy_class_planner.cpp

@@ -702,26 +702,19 @@ int HomotopyClassPlanner::bestTebIdx() const
   return -1;
 }
 
-bool HomotopyClassPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+int HomotopyClassPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
                                                 double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
 {
-  bool feasible = false;
-  while(rclcpp::ok() && !feasible && tebs_.size() > 0)
-  {
+  int feasible = 0;
+
     TebOptimalPlannerPtr best = findBestTeb();
     if (!best)
     {
       RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Couldn't retrieve the best plan");
-      return false;
+      return 0;
     }
     feasible = best->isTrajectoryFeasible(costmap_model, footprint_spec, inscribed_radius, circumscribed_radius, look_ahead_idx, feasibility_check_lookahead_distance);
-    if(!feasible)
-    {
-      removeTeb(best);
-      if(last_best_teb_ && (last_best_teb_ == best)) // Same plan as before.
-        return feasible;                             // Not failing could result in oscillations between trajectories.
-    }
-  }
+
   return feasible;
 }
 

teb_local_planner/src/optimal_planner.cpp

@@ -1266,7 +1266,7 @@ void TebOptimalPlanner::getFullTrajectory(std::vector<teb_msgs::msg::TrajectoryP
 }
 
 
-bool TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+int TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
                                              double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
 {
   if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses())
@@ -1291,7 +1291,7 @@ bool TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCriti
       {
         visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_);
       }
-      return false;
+      return i;
     }
     // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold
     // and interpolates in that case.
@@ -1318,20 +1318,20 @@ bool TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCriti
             {
               visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_);
             }
-            return false;
+            return i;
           }
         }
       }
     }
   }
-  return true;
+  return -1;
 }
 
 bool TebOptimalPlanner::isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model,
                            const std::vector<geometry_msgs::msg::Point>& footprint_spec)
 {
   try {
-    if ( costmap_model->scorePose(pose2d, dwb_critics::getOrientedFootprint(pose2d, footprint_spec)) < 0 ) {
+    if ( costmap_model->scorePose(pose2d, dwb_critics::getOrientedFootprint(pose2d, footprint_spec)) > 0 ) {
       return false;
     }
   } catch (...) {

teb_local_planner/src/teb_config.cpp

@@ -65,6 +65,8 @@ void TebConfig::declareParameters(const nav2_util::LifecycleNode::SharedPtr nh,
   declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_dist", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_dist));
   declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_angular", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_angular));
   declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_no_poses", rclcpp::ParameterValue(trajectory.feasibility_check_no_poses));
+  declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_stop_poses", rclcpp::ParameterValue(trajectory.feasibility_check_stop_poses));
+  declare_parameter_if_not_declared(nh, name + "." + "feasibility_check", rclcpp::ParameterValue(trajectory.feasibility_check));
   declare_parameter_if_not_declared(nh, name + "." + "publish_feedback", rclcpp::ParameterValue(trajectory.publish_feedback));
   declare_parameter_if_not_declared(nh, name + "." + "min_resolution_collision_check_angular", rclcpp::ParameterValue(trajectory.min_resolution_collision_check_angular));
   declare_parameter_if_not_declared(nh, name + "." + "control_look_ahead_poses", rclcpp::ParameterValue(trajectory.control_look_ahead_poses));
@@ -190,16 +192,22 @@ void TebConfig::loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::Share
   nh->get_parameter_or(name + "." + "force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist);
   nh->get_parameter_or(name + "." + "force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular);
   nh->get_parameter_or(name + "." + "feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses);
+  nh->get_parameter_or(name + "." + "feasibility_check_stop_poses", trajectory.feasibility_check_stop_poses, trajectory.feasibility_check_stop_poses);
+  nh->get_parameter_or(name + "." + "feasibility_check", trajectory.feasibility_check, trajectory.feasibility_check);
   nh->get_parameter_or(name + "." + "publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback);
   nh->get_parameter_or(name + "." + "min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular);
   nh->get_parameter_or(name + "." + "control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses);
   nh->get_parameter_or(name + "." + "feasibility_check_lookahead_distance", trajectory.feasibility_check_lookahead_distance, trajectory.feasibility_check_lookahead_distance);
 
   // Robot
   nh->get_parameter_or(name + "." + "max_vel_x", robot.max_vel_x, robot.max_vel_x);
+  nh->get_parameter_or(name + "." + "max_vel_x", robot.base_max_vel_x, robot.base_max_vel_x);
   nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
+  nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.base_max_vel_x_backwards, robot.base_max_vel_x_backwards);
   nh->get_parameter_or(name + "." + "max_vel_y", robot.max_vel_y, robot.max_vel_y);
+  nh->get_parameter_or(name + "." + "max_vel_y", robot.base_max_vel_y, robot.base_max_vel_y);
   nh->get_parameter_or(name + "." + "max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
+  nh->get_parameter_or(name + "." + "max_vel_theta", robot.base_max_vel_theta, robot.base_max_vel_theta);
   nh->get_parameter_or(name + "." + "acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
   nh->get_parameter_or(name + "." + "acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
   nh->get_parameter_or(name + "." + "acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
@@ -475,6 +483,8 @@ void TebConfig::on_parameter_event_callback(
         trajectory.max_samples = value.integer_value;
       } else if (name == node_name + ".feasibility_check_no_poses") {
         trajectory.feasibility_check_no_poses = value.integer_value;
+      }else if (name == node_name + ".feasibility_check_stop_poses") {
+        trajectory.feasibility_check_stop_poses = value.integer_value;
       } else if (name == node_name + ".control_look_ahead_poses") {
         trajectory.control_look_ahead_poses = value.integer_value;
       }
@@ -513,6 +523,8 @@ void TebConfig::on_parameter_event_callback(
         trajectory.exact_arc_length = value.bool_value;
       } else if (name == node_name + ".publish_feedback") {
         trajectory.publish_feedback = value.bool_value;
+      }else if (name == node_name + ".feasibility_check") {
+        trajectory.feasibility_check = value.bool_value;
       }
       // Robot
       else if (name == node_name + ".cmd_angle_instead_rotvel") {