check if given point is inside costmap

asked 2021-11-05 07:11:54 -0600

dinesh gravatar image

Here i have written small code segment to check if global path is inside costmap so that move base can update the global path. But this is not working properly.

double get_cost(costmap_2d::Costmap2DROS* cost_map_ros, const tf2::Vector3& world_pos)
{
    auto cost_map = cost_map_ros->getCostmap();

    int coords[2] = {};
    cost_map->worldToMapEnforceBounds(world_pos.x(), world_pos.y(), coords[0], coords[1]);

    return cost_map->getCost(coords[0], coords[1]) / 255.;
}
    // Check if global path is inside costmap, if yes, update the global path
    int max_cost = 0;
        std::vector<tf2::Transform> local_plan;
    for(auto pnt : local_plan) {
    auto cost = get_cost(m_cost_map, pnt.getOrigin());
    if(cost >= 0.99) {
        ROS_INFO("obstacle detected, %f, %ld", cost, local_plan.size());
        m_state = state_t::STATE_STUCK;
        return false;           
    }
}

But somehowe it is showing result that everytime global path is inside costmap even when global path is free.

edit retag flag offensive close merge delete