Getting collision distance field (like CHOMP) with moveit_experimental

[Alex Lavin]

Hi all,

I've been trying to extract the signed distance field (SDF) functionality from CHOMP -- I simply want to write a method `PlanningScene::CheckSDF` that gives me a vector of distance values from each robot sphere to its nearest obstacle. With the following code I came across this old MoveIt issue: https://github.com/ros-planning/moveit/issues/15.

```

    const collision_detection::CollisionWorldHybrid* hy_world;

    const collision_detection::CollisionRobotHybrid* hy_robot;

    hy_world =

        dynamic_cast< const collision_detection::CollisionWorldHybrid* >(

            planning_scene_->getCollisionWorld(planning_scene_->getActiveCollisionDetectorName()).get());

    if (!hy_world) {

        std::cerr << "Could not initialize hybrid collision world from planning scene" << std::endl;

        return false;

    }

    hy_robot = dynamic_cast<const collision_detection::CollisionRobotHybrid*>(

        planning_scene_->getCollisionRobot(planning_scene_->getActiveCollisionDetectorName()).get()

    );

    if (!hy_robot) {

        std::cerr << "Could not initialize hybrid collision robot from planning scene" << std::endl;

        return false;

    }

    auto req = collision_detection::CollisionRequest();

    auto res = collision_detection::CollisionResult();

    moveit::core::RobotState state(robot_model_);

    collision_detection::GroupStateRepresentationPtr gsr;

    auto robot = *hy_robot->getCollisionRobotDistanceField().get();

    hy_world->checkCollisionDistanceField(req, res, robot, state, gsr);  // Passthrough to `cworld_distance_->checkCollision(req, res, robot, state, gsr);`

    // Debug lines to see what's going on:

    int num_collision_points_ = 0;

    std::map<std::string, std::string> fixed_link_resolution_map;

    std::cout << "Num grads = "<< gsr->gradients_.size() << std::endl;

    for (auto g = 0; g < gsr->gradients_.size(); g++)

    {

        num_collision_points_ += gsr->gradients_[g].gradients.size();

        collision_detection::GradientInfo& info = gsr->gradients_[g];

        std::vector<double> distances = info.distances;

        std::cout << distances.size() << std::endl;

    }

```

which gives me the `Could not initialize hybrid collision world from planning scene` error message. 

Also note that I can use `PlanningScene::CheckCollision` and then look at the `CollisionResult` for the distance value, but this only gives me one distance value for the full robot.

I'm using the collision_distance_field package from moveit_experimental. Any help you can offer me would be greatly appreciated!

Cheers,

Alex