ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Make global path update correctly when it is invalid

asked 2021-12-27 00:56:28 -0500

dinesh gravatar image

updated 2021-12-27 00:58:38 -0500

/* * 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;   
        }
    }
}

image description 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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-12-28 01:48:23 -0500

dinesh gravatar image

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.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2021-12-27 00:56:28 -0500

Seen: 67 times

Last updated: Dec 28 '21