PlanningScene::distanceToCollision returning large (possibly uninitialized) distances

[scottpaulin]

Hi,

I am getting some very large distances when using distanceToCollision. I have compared these to distances computed using checkCollision with distance set in the CollisionRequest. I am using the Universal Robot package on Indigo.

Output:

valid: 1 distance_checkCollision: 0.0148276 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0145733 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0147373 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0140031 distance_distanceToCollision: 1.79769e+308
valid: 0 distance_checkCollision: 0         distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0148941 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0083631 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0146611 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0142101 distance_distanceToCollision: 1.79769e+308
valid: 1 distance_checkCollision: 0.0146178 distance_distanceToCollision: 1.79769e+308

Code:

robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); 
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene scene(kinematic_model);

robot_state::RobotState state = scene.getCurrentState();
const int attempts = 10;
for (int i = 0; i < attempts; ++i)
{
    state.setToRandomPositions();
    state.update(true);
    collision_detection::CollisionRequest request;
    request.distance = true;

    collision_detection::CollisionResult result;
    result.clear();
    scene.checkCollision(request, result, state);
    const bool valid = result.collision == false;
    double distance_checkCollision = result.distance;
    double distance_distanceToCollision = scene.distanceToCollision(state);
    ROS_ERROR_STREAM("valid: " << valid << " distance_checkCollision: " << distance_checkCollision << " distance_distanceToCollision: " << distance_distanceToCollision);
}

Regards,

Scott.

[mikeferguson]

ported to new repo