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

fixed wrong state access in feasibility check

master
Christoph Rösmann 2020-07-19 14:33:42 +02:00
parent 0ad611113c
commit 8a5980a7ae
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mpc_local_planner/src/controller.cpp
View File

@ -891,7 +891,7 @@ bool Controller::isPoseTrajectoryFeasible(base_local_planner::CostmapModel* cost
// (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)!
if (i < look_ahead_idx) if (i < look_ahead_idx)
{ {
double delta_rot = normalize_theta(fd_grid->getState(i)[i + 1] - fd_grid->getState(i)[0]); double delta_rot = normalize_theta(fd_grid->getState(i + 1)[2] - fd_grid->getState(i)[2]);
Eigen::Vector2d delta_dist = fd_grid->getState(i + 1).head(2) - fd_grid->getState(i).head(2); Eigen::Vector2d delta_dist = fd_grid->getState(i + 1).head(2) - fd_grid->getState(i).head(2);
if (std::abs(delta_rot) > min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) if (std::abs(delta_rot) > min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius)
{ {