hg clone https://bitbucket.org/osrf/collision_map_creator_plugin

package collision_map_creator_msgs.msgs;
import "vector2d.proto";

message CollisionMapRequest
{
  required gazebo.msgs.Vector2d   upperLeft  = 1;
  required gazebo.msgs.Vector2d   upperRight = 2;
  required gazebo.msgs.Vector2d   lowerRight = 3;
  required gazebo.msgs.Vector2d   lowerLeft  = 4;
  required double                 height     = 5;
  required double                 resolution = 6;
  optional string                 filename   = 7 [default = ""];
  optional int32                  threshold  = 8 [default = 255];
}

package collision_map_creator_msgs.msgs;

import "vector2d.proto";

message CollisionMapRequest{}

["optional"/"required"] [field type] [field name] = [enum];

find_package(Protobuf REQUIRED)

set(PROTOBUF_IMPORT_DIRS)
foreach(ITR ${GAZEBO_INCLUDE_DIRS})
  if(ITR MATCHES ".*gazebo-[0-9.]+$")
    set(PROTOBUF_IMPORT_DIRS "${ITR}/gazebo/msgs/proto")
  endif()
endforeach()

set (msgs
  collision_map_request.proto
  ${PROTOBUF_IMPORT_DIRS}/vector2d.proto
  ${PROTOBUF_IMPORT_DIRS}/header.proto
  ${PROTOBUF_IMPORT_DIRS}/time.proto
)

add_library(collision_map_creator_msgs SHARED ${PROTO_SRCS})
target_link_libraries(collision_map_creator_msgs ${PROTOBUF_LIBRARY})

#include <iostream>
#include <math.h>
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <boost/shared_ptr.hpp>        
#include <sdf/sdf.hh>
#include <ignition/math/Vector3.hh>

#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "collision_map_request.pb.h"

namespace gazebo
{
typedef const boost::shared_ptr<
  const collision_map_creator_msgs::msgs::CollisionMapRequest>
    CollisionMapRequestPtr;

class CollisionMapCreator : public WorldPlugin
{
  transport::NodePtr node;
  transport::PublisherPtr imagePub;
  transport::SubscriberPtr commandSubscriber;
  physics::WorldPtr world;

  public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
  {
    node = transport::NodePtr(new transport::Node());
    world = _parent;
    // Initialize the node with the world name
    node->Init(world->Name());
    std::cout << "Subscribing to: " << "~/collision_map/command" << std::endl;
    commandSubscriber = node->Subscribe("~/collision_map/command",
      &CollisionMapCreator::create, this);
    imagePub = node->Advertise<msgs::Image>("~/collision_map/image");
  }

  public: void create(CollisionMapRequestPtr &msg)
  {
    std::cout << "Received message" << std::endl;

    std::cout << "Creating collision map with corners at (" <<
      msg->upperleft().x() << ", " << msg->upperleft().y() << "), (" <<
      msg->upperright().x() << ", " << msg->upperright().y() << "), (" <<
      msg->lowerright().x() << ", " << msg->lowerright().y() << "), (" <<
      msg->lowerleft().x() << ", " << msg->lowerleft().y() <<
        ") with collision projected from z = " <<
      msg->height() << "\nResolution = " << msg->resolution() << " m\n" <<
        "Occupied spaces will be filled with: " << msg->threshold() <<
        std::endl;

    double dX_vertical = msg->upperleft().x() - msg->lowerleft().x();
    double dY_vertical = msg->upperleft().y() - msg->lowerleft().y();
    double mag_vertical =
      sqrt(dX_vertical * dX_vertical + dY_vertical * dY_vertical);
    dX_vertical = msg->resolution() * dX_vertical / mag_vertical;
    dY_vertical = msg->resolution() * dY_vertical / mag_vertical;

    double dX_horizontal = msg->upperright().x() - msg->upperleft().x();
    double dY_horizontal = msg->upperright().y() - msg->upperleft().y();
    double mag_horizontal =
      sqrt(dX_horizontal * dX_horizontal + dY_horizontal * dY_horizontal);
    dX_horizontal = msg->resolution() * dX_horizontal / mag_horizontal;
    dY_horizontal = msg->resolution() * dY_horizontal / mag_horizontal;

    int count_vertical = mag_vertical / msg->resolution();
    int count_horizontal = mag_horizontal / msg->resolution();

    if (count_vertical == 0 || count_horizontal == 0)
    {
      std::cout << "Image has a zero dimensions, check coordinates"
                << std::endl;
      return;
    }
    double x,y;

    boost::gil::gray8_pixel_t fill(255-msg->threshold());
    boost::gil::gray8_pixel_t blank(255);
    boost::gil::gray8_image_t image(count_horizontal, count_vertical);

    double dist;
    std::string entityName;
    ignition::math::Vector3d start, end;
    start.Z(msg->height());
    end.Z(0.001);

    gazebo::physics::PhysicsEnginePtr engine = world->Physics();
    engine->InitForThread();
    gazebo::physics::RayShapePtr ray =
      boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
        engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

    std::cout << "Rasterizing model and checking collisions" << std::endl;
boost::gil::fill_pixels(image._view, blank);

    for (int i = 0; i < count_vertical; ++i)
    {
  std::cout << "Percent complete: " << i * 100.0 / count_vertical
            << std::endl;
  x = i * dX_vertical + msg->lowerleft().x();
  y = i * dY_vertical + msg->lowerleft().y();
      for (int j = 0; j < count_horizontal; ++j)
  {
    x += dX_horizontal;
    y += dY_horizontal;

    start.X(x);
    end.X(x);
    start.Y(y);
    end.Y(y);
    ray->SetPoints(start, end);
    ray->GetIntersection(dist, entityName);
    if (!entityName.empty())
    {
      image._view(i,j) = fill;
    }
  }
}

std::cout << "Completed calculations, writing to image" << std::endl;
if (!msg->filename().empty())
{
  boost::gil::gray8_view_t view = image._view;
  boost::gil::png_write_view(msg->filename(), view);
    }
  }
};

// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(CollisionMapCreator)
}

#include <iostream>
#include <math.h>
#include <boost/shared_ptr.hpp>

#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "collision_map_request.pb.h"

#include "collision_map_request.pb.h"

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>

namespace gazebo
{

typedef const boost::shared_ptr<const collision_map_creator_msgs::msgs::CollisionMapRequest>             CollisionMapRequestPtr;

public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
{
  node = transport::NodePtr(new transport::Node());
  world = _parent;
  // Initialize the node with the world name
  node->Init(world->GetName());
  std::cout << "Subscribing to: " << "~/collision_map/command" << std::endl;

    commandSubscriber = node->Subscribe("~/collision_map/command", &CollisionMapCreator::create, this);
    imagePub = node->Advertise<msgs::Image>("~/collision_map/image");

public: void create(CollisionMapRequestPtr &msg)

    double z = msg->height();

    double dX_vertical = msg->upperleft().x() - msg->lowerleft().x();
    double dY_vertical = msg->upperleft().y() - msg->lowerleft().y();
    double mag_vertical = sqrt(dX_vertical * dX_vertical + dY_vertical * dY_vertical);
    dX_vertical = msg->resolution() * dX_vertical / mag_vertical;
    dY_vertical = msg->resolution() * dY_vertical / mag_vertical;

    gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
    engine->InitForThread();
    gazebo::physics::RayShapePtr ray = boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
          engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

    for (int i = 0; i < count_vertical; ++i)
    {

    msgs::Image image;
    image.set_width(count_horizontal);
    image.set_height(count_vertical);
    image.set_pixel_format(0);
    image.set_step(count_horizontal);
    image.set_data(data);

    imagePub->Publish(image);

string cornersStr = vectorString;
size_t opening=0;
size_t closing=0;
for (it = corners.begin(); it != corners.end(); ++it)
{
    opening = cornersStr.find('(', closing);
    closing = cornersStr.find(')', opening);
    if (opening == string::npos || closing == string::npos)
    {
        std::cout << "Poorly formed string: " << cornersStr << std::endl;
        std::cout << "( found at: " << opening << " ) found at: " << closing << std::endl;
        return false;
    }
    string oneCornerStr = cornersStr.substr(opening + 1, closing - opening - 1);
    size_t commaLoc = oneCornerStr.find(",");
    string x = oneCornerStr.substr(0,commaLoc);
    string y = oneCornerStr.substr(commaLoc + 1, oneCornerStr.length() - commaLoc);
    (*it)->set_x(atof(x.c_str()));
    (*it)->set_y(atof(y.c_str()));
}
return true;

    corners.push_back(request.mutable_upperleft());
    corners.push_back(request.mutable_upperright());
    corners.push_back(request.mutable_lowerright());
    corners.push_back(request.mutable_lowerleft());

    if (!createVectorArray(argv[1],corners))
    {
        return -1;
    }

    request.set_height(atof(argv[2]));
    request.set_resolution(atof(argv[3]));
    request.set_filename(argv[4]);

    if (argc == 6)
    {
        request.set_threshold(atoi(argv[5]));
    }

    gazebo::transport::init();
    gazebo::transport::run();
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init("default");

    std::cout << "Request: " <<
                 " UL.x: " << request.upperleft().x() <<
                 " UL.y: " << request.upperleft().y() <<
                 " UR.x: " << request.upperright().x() <<
                 " UR.y: " << request.upperright().y() <<
                 " LR.x: " << request.lowerright().x() <<
                 " LR.y: " << request.lowerright().y() <<
                 " LL.x: " << request.lowerleft().x() <<
                 " LL.y: " << request.lowerleft().y() <<
                 " Height: " << request.height() <<
                 " Resolution: " << request.resolution() <<
                 " Filename: " << request.filename() <<
                 " Threshold: " << request.threshold() << std::endl;

    gazebo::transport::PublisherPtr imagePub =
            node->Advertise<collision_map_creator_msgs::msgs::CollisionMapRequest>(
                                                        "~/collision_map/command");
    imagePub->WaitForConnection();
    imagePub->Publish(request);

    gazebo::transport::fini();
    return 0;
}
return -1;

    corners.push_back(request.mutable_upperleft());
    corners.push_back(request.mutable_upperright());
    corners.push_back(request.mutable_lowerright());
    corners.push_back(request.mutable_lowerleft());

    request.set_height(atof(argv[2]));
    request.set_resolution(atof(argv[3]));
    request.set_filename(argv[4]);

    gazebo::transport::init();
    gazebo::transport::run();
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init("default");

    gazebo::transport::PublisherPtr imagePub =
            node->Advertise<collision_map_creator_msgs::msgs::CollisionMapRequest>(
                                                        "~/collision_map/command");

    imagePub->WaitForConnection();
    imagePub->Publish(request);

   <include>
        <uri>model://sun</uri>
      </include>

   <include>
     <uri>model://willowgarage</uri>
   </include>

   <plugin filename="libcollision_map_creator.so" name="collision_map_creator"/>
 </world>