global path not updating

asked 2021-12-20 00:38:40 -0600

dinesh gravatar image

updated 2021-12-20 00:38:57 -0600

image description

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.

edit retag flag offensive close merge delete