-
Notifications
You must be signed in to change notification settings - Fork 8
/
request_publisher.cc
111 lines (95 loc) · 3.64 KB
/
request_publisher.cc
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
#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 "collision_map_request.pb.h"
#include "vector2d.pb.h"
using namespace std;
bool createVectorArray(const char * vectorString,
deque<gazebo::msgs::Vector2d*> corners)
{
deque<gazebo::msgs::Vector2d*>::iterator it;
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;
}
int main(int argc, char * argv[])
{
if (argc > 4)
{
collision_map_creator_msgs::msgs::CollisionMapRequest request;
deque<gazebo::msgs::Vector2d*> corners;
corners.push_back(request.mutable_lowerleft());
corners.push_back(request.mutable_upperright());
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]));
}
if (argc >= 7)
{
request.set_groundentityname(argv[6]);
}
if (argc >= 8)
{
request.set_minheight(atof(argv[7]));
}
gazebo::transport::init();
gazebo::transport::run();
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init("default");
std::cout << "Request: " <<
" LL.x: " << request.lowerleft().x() <<
" LL.y: " << request.lowerleft().y() <<
" UR.x: " << request.upperright().x() <<
" UR.y: " << request.upperright().y() <<
" Height: " << request.height() <<
" Resolution: " << request.resolution() <<
" Filename: " << request.filename() <<
" Threshold: " << request.threshold() <<
" GroundEntityName: " << request.groundentityname() <<
" MinHeight: " << request.minheight() << 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;
} else {
std::cerr << "Usage: " << argv[0] <<
" \"(lowerleft.x,lowerleft.y)(upperright.x,upperright.y)\"" <<
" maxheight resolution outfile.png [threshold] [groundentityname] [minheight]" << std::endl;
return -1;
}
}