How to get footprintcost with base_local_planner::CostmalModel

edit

Someone familiar with implementing own local planner? I'm developing a follow the carrot local planner which also checks for collision with obstacles. To check collision, I tried two apporaches:

1. use costmap->getCost method to get costs of a single point (works good so far)
2. use base_local_planner::CostmapModel->footprintcost

The first one works good so far, however it doesn't take footprint of robot into account for sure. The last one either returns a collission (-1), if there is no obstacle. Also it doesn't return collisions, if robots footprint already crashed into something.

As most planners out there seems to use CostmapModel, I'm promise that it should work. But why doesn't it work on my attemps? To my best knowledge, CostmapModel uses world coordinates, so there is no need to transform the points of footprint into costmap coordinates. The red square shows the point where the planner finds a collision.

May I ask someone of you to have a look at my code here? FTC_LOCAL_PLANNER

During initialization, I get costmap and footprint like this:

// Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
    footprint_spec_ = costmap->getRobotFootprint();
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius_);
    costmap_model_ = new base_local_planner::CostmapModel(*costmap_map_);

right before checking the feasability of the path, I also tried to update to the latest data with no success. getRobotFootprint returns footprint in world coordinates according to documentation. So I wonder if I need to refresh it always

footprint_spec_ = costmap->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius_);
        if (!isTrajectoryFeasible(costmap_model_, footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius_, 5))
        {
            cmd_vel.twist.linear.x = 0;
            cmd_vel.twist.angular.z = 0;
            is_crashed = true;
            return RET_BLOCKED;
        }

and finally, this is how I try to get the costs

double costs = costmap_model->footprintCost(x_pose.pose.position, footprint_spec, inscribed_radius, circumscribed_radius);

This always returns either -3, which means footprint off the costmap (can't imagine how this can be possible in a x2m map with offset of -1,-1). This results also in crashes into obstacles as footprint is off the map.

Or it returns -1, which means collision if there is no obstacle.