global path not updating
When local planner is requesting move base to update the global path when global path is invalid, it is not updating.
/**
* Segment 1: If global path is invalid, update the global path
**/
for(auto pnt : local_plan) {
double map_resolution = 0.1;
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;
}
}
}
Here this code segment checkes the gloal path, if it is found invalid i.e not passed from free space it returns false witgh move base state as STATE_STUCK. But the global path planner is again calculating the same global path.
Asked by dinesh on 2021-12-20 01:38:40 UTC
Comments