feat: collision

master
邱棚 2024-07-16 10:41:10 +08:00
parent dee3c6856b
commit 977eeebf89
2 changed files with 8 additions and 4 deletions

View File

@ -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

View File

@ -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