RoboMaster/RoboRTS

something wring in teb_local_planner.cpp

Closed this issue · 5 comments

lyziew commented

in teb_local_planner.cpp
i try print 'robot_inscribed_radius_' and 'robot_circumscribed_radius'
bool feasible = optimal_->IsTrajectoryFeasible(teb_error_info_, robot_cost_.get(), robot_footprint_, robot_inscribed_radius_, robot_circumscribed_radius, fesiable_step_look_ahead_); ROS_INFO("Footprint %f, %f", robot_inscribed_radius_, robot_circumscribed_radius);
the result is that
Selection_007
so i am sure robot_inscribed_radius_ and robot_circumscribed_radius calculate wrong

XYvven commented

hi，guy. this two value are not used for feasible checking(because the robot footprint I used is point), and I intialize the robot_inscribed_radius_ with std::numeric_limits::max() initialize the robot_circumscribed_radius with 0.0. so when you print out it is what you see in the picture.

lyziew commented

谢谢你的回复，我尝试添加dynamic obstacle 功能添加到现有框架上，然后发现我修改后的代码一直出现 trajectory is not feasible的错误，于是尝试打印这两个值查看一下，发现一个很大的值误以为是没有初始化的量，我在仔细检查一下footprint初始化那部分代码，再次感谢您的回复

lyziew commented

我看代码时没仔细看，从costmap里取到的footprint并没有用到而是使用了机器人位置的点作为footprint，这里为什么使用点模型而不使用多边形，我使用的是从costmap中获取的polygon(四个点),但是效果不好，一直会轨迹不可达，很迷

XYvven commented

因为在实际从world到map，会有一些精度损失，而在优化中设置的与障碍物的距离很近，这些精度损失就会导致实际在costmap中查询四边形的点的时候导致not feasible，另外在costmap的配置文件中，inflation_layer_config_min.prototxt里面的inflation值是直径，如果你使用四个点的话就是在c space里面使用多边形，那肯定会not feasible。所以实际情况中就使用了point计算

  • 👍1
lyziew commented

I get it, thanks very much