12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: collision

master
邱棚 2024-07-16 10:41:10 +08:00
parent dee3c6856b
commit 977eeebf89
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
mpc_local_planner/src/controller.cpp
View File

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

2
mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml
View File

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