feat: collision

mpc_local_planner/src/controller.cpp

@@ -58,12 +58,15 @@ namespace mpc_local_planner {
 bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles,
                            teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector<teb_local_planner::PoseSE2>& via_points)
 {
+
+    //创建机器模型
     _dynamics = configureRobotDynamics(nh);
     if (!_dynamics) return false;  // we may need state and control dimensions to check other parameters
-
+    //离散网络，比如多重打靶法。参考点，输入，状态，等变量也会存放在grid里面，会实时更新。而且grid也继承了顶点传入到超图问题构建中
     _grid   = configureGrid(nh);
+    //求解器
     _solver = configureSolver(nh);
-
+    //最优化问题构建, _dynamics，_grid，_solver这三个指针也会传入到最优化问题类里
     _structured_ocp = configureOcp(nh, obstacles, robot_model, via_points);
     _ocp            = _structured_ocp;  // copy pointer also to parent member
 
@@ -876,7 +879,7 @@ bool Controller::isPoseTrajectoryFeasible(base_local_planner::CostmapModel* cost
         ROS_ERROR("isPoseTrajectoriyFeasible is currently only implemented for fd grids");
         return true;
     }
-
+    ROS_INFO(" look_ahead_idx:%d",look_ahead_idx);
     if (look_ahead_idx < 0 || look_ahead_idx >= _grid->getN()) look_ahead_idx = _grid->getN() - 1;
 
     for (int i = 0; i <= look_ahead_idx; ++i)
@@ -884,6 +887,7 @@ bool Controller::isPoseTrajectoryFeasible(base_local_planner::CostmapModel* cost
         if (costmap_model->footprintCost(fd_grid->getState(i)[0], fd_grid->getState(i)[1], fd_grid->getState(i)[2], footprint_spec, inscribed_radius,
                                          circumscribed_radius) == -1)
         {
+            ROS_INFO("Idx:%d Pose is not feasiable!,lookaheadidx:%d",i,look_ahead_idx);
             return false;
         }
         // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold

mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml

@@ -30,7 +30,7 @@ MpcLocalPlannerROS:
 
 
   ## Collision avoidance
-  collision_avoidance:
+  collision:
     min_obstacle_dist: 0.27 # Note, this parameter must be chosen w.r.t. the footprint_model
     enable_dynamic_obstacles: False
     force_inclusion_dist: 0.5