Make global path update correctly when it is invalid
/* * Segment 1: If global path is invalid, update the global path */
for(auto pnt : local_plan) {
double map_resolution = 0.05;
int free_cell = 0;
int count = 0;
double pnt_dist = sqrt(pow(pnt.getOrigin().x() - local_pose.getOrigin().x(), 2) + pow(pnt.getOrigin().y() - local_pose.getOrigin().y(), 2));
if(pnt_dist < local_costmap_height/2.0) {
for(double x=pnt.getOrigin().x()-map_resolution;x<=pnt.getOrigin().x()+map_resolution;x+=map_resolution) {
for(double y=pnt.getOrigin().y()-map_resolution;y<=pnt.getOrigin().y()+map_resolution;y+=map_resolution) {
geometry_msgs::Pose gp;
gp.position.x = x;
gp.position.y = y;
tf2::Transform p;
tf2::fromMsg(gp, p);
double cost = get_cost(m_cost_map, p.getOrigin());
// ROS_INFO("count: %d, cost: %f", ++count, cost);
if(cost < m_max_cost) {
free_cell++;
}
}
}
// ROS_INFO("free_cell: %d", free_cell);
if(free_cell==0) {
ROS_WARN("No valid local path found!");
m_state = state_t::STATE_STUCK;
return false;
}
}
}
When given global pathg is invalid i.e passes from local cosmaps this code segment is calculating it as invalid. Here even after the local planner returns false move base global planner i.e navfn is not creating the valid global path. How to resolve this issue?