地図を生成するGazebo Pluginを作る

以下のコード以外は Custom messages にあるものと同じです。

map_builder.cpp

map_builder.cpp
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
#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 "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 "map_request.pb.h"

#include "yaml-cpp/yaml.h"
#include <ros/ros.h>
#include <fstream>
#include <boost/filesystem.hpp>

namespace gazebo
{
typedef const boost::shared_ptr<const dronedoc::msgs::MapRequest> MapRequestPtr;

class MapCreator : 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->GetName());
    ROS_INFO("Subscribing to: ~/collision_map/command");
    commandSubscriber = node->Subscribe("~/collision_map/command",
      &MapCreator::create, this);
    imagePub = node->Advertise<msgs::Image>("~/collision_map/image");
  }

  public: void create(MapRequestPtr &msg)
  {
    ROS_INFO("Received message");

    ROS_INFO("Creating collision map with origin at (%f, %f), width %f and height %f "
              "with collision projected from z = %f\n"
              "Resolution = %f m\n"
              "Occupied spaces will be filled with: %d",
             msg->origin().x(),
             msg->origin().y(),
             msg->map_width(),
             msg->map_height(),
             msg->height(),
             msg->resolution(),
             msg->threshold());

    int count_vertical = msg->map_height() / msg->resolution();
    int count_horizontal = msg->map_width() / msg->resolution();

    if (count_vertical == 0 || count_horizontal == 0)
    {
      ROS_ERROR("Image has a zero dimensions, check coordinates");
      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;
    math::Vector3 start, end;
    start.z = msg->height();
    end.z = 0.001;

    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()));

    ROS_INFO("Rasterizing model and checking collisions");
    boost::gil::fill_pixels(image._view, blank);

    int progress = 0;
    for (int i = 0; i < count_vertical; ++i)
    {
      if(progress != std::round(i * 100.0 / count_vertical)){
        ROS_INFO("%d%% complete", progress);
      }
      progress = std::round(i * 100.0 / count_vertical);
      x = msg->origin().x();
      y = i * msg->resolution() + msg->origin().y();
      for (int j = 0; j < count_horizontal; ++j)
      {
        x += msg->resolution();

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

    boost::filesystem::path fname(msg->filename());
    fname = boost::filesystem::absolute(fname);

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

    ROS_INFO("Writing map info");

    YAML::Emitter out;
    out << YAML::BeginMap;
    out << YAML::Key << "image";
    out << YAML::Value << fname.filename().string();
    out << YAML::Key << "resolution";
    out << YAML::Value << msg->resolution();
    out << YAML::Key << "negate";
    out << YAML::Value << 0;
    out << YAML::Key << "free_thresh";
    out << YAML::Value << 0.196;
    out << YAML::Key << "occupied_thresh";
    out << YAML::Value << 0.65;
    out << YAML::Key << "origin";
    out << YAML::Value;
    out << YAML::Flow;
    out << YAML::BeginSeq << msg->origin().x() << msg->origin().y() << 0.00 << YAML::EndSeq;

    std::ofstream yaml_out(fname.replace_extension(".yaml").string());
    yaml_out << out.c_str();
    yaml_out.close();

    ROS_INFO("Map built successfully");
  }
};

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

request_publisher.cpp

request_publisher.cpp
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
#include <iostream>
#include <math.h>
#include <deque>
#include <sdf/sdf.hh>

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

#include "map_request.pb.h"
#include "vector2d.pb.h"

#include <ros/ros.h>
#include <boost/filesystem/path.hpp>

using namespace std;

bool createVector(const char * vectorString,
                       gazebo::msgs::Vector2d* origin)
{
    string cornersStr = vectorString;
    size_t opening=0;
    size_t closing=0;

    opening = cornersStr.find('(', closing);
    closing = cornersStr.find(')', opening);
    if (opening == string::npos || closing == string::npos)
    {
        ROS_ERROR("Poorly formed string: %s", cornersStr.c_str());
        ROS_ERROR("( found at: %ld ) found at: %ld", opening, closing);
        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);
    origin->set_x(atof(x.c_str()));
    origin->set_y(atof(y.c_str()));

    return true;
}

int main(int argc, char * argv[])
{
    if (argc > 6)
    {
        dronedoc::msgs::MapRequest request;
        gazebo::msgs::Vector2d* origin = request.mutable_origin();

        if (!createVector(argv[3], origin))
        {
            ROS_ERROR("Failed to create rasterize region.");
            return -1;
        }


        request.set_height(atof(argv[1]));
        request.set_resolution(atof(argv[2]));
        request.set_map_width(atof(argv[4]));
        request.set_map_height(atof(argv[5]));

        std::string filename;
        if(argc < 7){
            filename = "map.png";
        } else {
            filename = argv[6];
        }
        boost::filesystem::path fname(filename);
        fname = boost::filesystem::absolute(fname);
        request.set_filename(fname.string());

        if (argc == 8)
        {
            request.set_threshold(atoi(argv[7]));
        }

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


        ROS_INFO("\nRequest: \n"
                 " origin.x: %f origin.y: %f\n"
                 " map_width: %f map_height: %f\n"
                 " Height: %f\n Resolution: %f\n Filename: %s\n Threshold: %d",
                 request.origin().x(),
                 request.origin().y(),
                 request.map_width(),
                 request.map_height(),
                 request.height(),
                 request.resolution(),
                 request.filename().c_str(),
                 request.threshold());

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

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

    std::cout << "Usage: " <<
                 "rosrun dronedoc request_publisher a1 a2 a3 a4 a5 [a6 a7]\n" <<
                 "\ta1: height\n" <<
                 "\ta2: resolution\n" <<
                 "\ta3: \"(origin.x, origin.y)\"\n" <<
                 "\t  Origin is the point on lower left corner of map image\n"
                 "\ta4: map_width\n" <<
                 "\ta5: map_height\n" <<
                 "\ta6: filename\t[default=\"map\"]\n" <<
                 "\ta7: threshold\t[default=255]\n" << std::endl;

    return -1;
}

使用法

プラグインを埋め込んだワールドを起動した状態で、次のようにノードを実行することでマップを作成できます。 以下のコマンドは一例です。引数無しで実行すれば使用法が表示されるので参考にしてください。

rosrun dronedoc request_publisher 10 0.01 "(-12,0)" 60 50 ~/map.png 255

参考

Custom messages

カスタムメッセージを使ってコリジョンマップを作る

ROS Plugin

ROS用のGazeboプラグインを作る