From 977eeebf8919b35daf5d13e2c7369f0922529e06 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Tue, 16 Jul 2024 10:41:10 +0800 Subject: [PATCH] feat: collision --- mpc_local_planner/src/controller.cpp | 10 +++++++--- .../cfg/carlike/mpc_local_planner_params.yaml | 2 +- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp index 04de144..dd42a17 100644 --- a/mpc_local_planner/src/controller.cpp +++ b/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& 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 diff --git a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml index b6bf30c..ff7714a 100644 --- a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml +++ b/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