Make global path update correctly when it is invalid

Question

/** * 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?

---

Originally posted by dinesh on ROS Answers with karma: 932 on 2021-12-27

Post score: 0

Answer 1

The bug was that due to global costmap not being in full intensity like local costmap, even after base_local_planner requested the move_base to update the global path, it was not updated correctly, since the global costmap was not in correct form due to less intense.

---

Originally posted by dinesh with karma: 932 on 2021-12-28

This answer was ACCEPTED on the original site

Post score: 1