g2o
test_simulator2d.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #include <cstdlib>
28 #include "g2o/stuff/command_args.h"
29 #include "g2o/stuff/sampler.h"
30 #include "simulator2d.h"
32 #include <iostream>
33 #include <fstream>
34 #include <sstream>
35 
36 using namespace g2o;
37 using namespace std;
38 using namespace Eigen;
39 
40 int main(int argc, char** argv) {
41  CommandArgs arg;
42  int nlandmarks;
43  int simSteps;
44  double worldSize;
45  bool hasOdom;
46  bool hasPoseSensor;
47  bool hasPointSensor;
48  bool hasPointBearingSensor;
49  bool hasCompass;
50  bool hasGPS;
51 
52  bool hasSegmentSensor;
53  int nSegments;
54  int segmentGridSize;
55  double minSegmentLenght, maxSegmentLenght;
56 
57 
58  std::string outputFilename;
59  arg.param("nlandmarks", nlandmarks, 100, "number of landmarks in the map");
60  arg.param("nSegments", nSegments, 1000, "number of segments");
61  arg.param("segmentGridSize", segmentGridSize, 50, "number of cells of the grid where to align the segments");
62  arg.param("minSegmentLenght", minSegmentLenght, 0.5, "minimal lenght of a segment in the world");
63  arg.param("maxSegmentLenght", maxSegmentLenght, 3, "maximal lenght of a segment in the world");
64 
65  arg.param("simSteps", simSteps, 100, "number of simulation steps");
66  arg.param("worldSize", worldSize, 25.0, "size of the world");
67  arg.param("hasOdom", hasOdom, false, "the robot has an odometry" );
68  arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
69  arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
70  arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" );
71  arg.param("hasCompass", hasCompass, false, "the robot has a compass");
72  arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
73  arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
74  arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);
75 
76 
77  arg.parseArgs(argc, argv);
78 
79  std::mt19937 generator;
81  World world(&graph);
82  for (int i=0; i<nlandmarks; i++){
83  WorldObjectPointXY * landmark = new WorldObjectPointXY;
84  double x = sampleUniform(-.5,.5, &generator)*(worldSize+5);
85  double y = sampleUniform(-.5,.5, &generator)*(worldSize+5);
86  landmark->vertex()->setEstimate(Vector2d(x,y));
87  world.addWorldObject(landmark);
88  }
89 
90  cerr << "nSegments = " << nSegments << endl;
91 
92  for (int i=0; i<nSegments; i++){
94  int ix = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
95  int iy = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
96  int ith = sampleUniform(0,3, &generator);
97  double th= (M_PI/2)*ith;
98  th=atan2(sin(th),cos(th));
99  double xc = ix*(worldSize/segmentGridSize);
100  double yc = iy*(worldSize/segmentGridSize);
101 
102  double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator);
103 
104  double x1 = xc + cos(th)*l2;
105  double y1 = yc + sin(th)*l2;
106  double x2 = xc - cos(th)*l2;
107  double y2 = yc - sin(th)*l2;
108 
109 
110  segment->vertex()->setEstimateP1(Vector2d(x1,y1));
111  segment->vertex()->setEstimateP2(Vector2d(x2,y2));
112  world.addWorldObject(segment);
113  }
114 
115 
116 
117 
118 
119  Robot2D robot(&world, "myRobot");
120  world.addRobot(&robot);
121 
122 
123  stringstream ss;
124  ss << "-ws" << worldSize;
125  ss << "-nl" << nlandmarks;
126  ss << "-steps" << simSteps;
127 
128  if (hasOdom) {
129  SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
130  robot.addSensor(odometrySensor);
131  Matrix3d odomInfo = odometrySensor->information();
132  odomInfo.setIdentity();
133  odomInfo*=500;
134  odomInfo(2,2)=5000;
135  odometrySensor->setInformation(odomInfo);
136  ss << "-odom";
137  }
138 
139  if (hasPoseSensor) {
140  SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
141  robot.addSensor(poseSensor);
142  Matrix3d poseInfo = poseSensor->information();
143  poseInfo.setIdentity();
144  poseInfo*=500;
145  poseInfo(2,2)=5000;
146  poseSensor->setInformation(poseInfo);
147  ss << "-pose";
148  }
149 
150  if (hasPointSensor) {
151  SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
152  robot.addSensor(pointSensor);
153  Matrix2d pointInfo = pointSensor->information();
154  pointInfo.setIdentity();
155  pointInfo*=1000;
156  pointSensor->setInformation(pointInfo);
157  pointSensor->setFov(0.75*M_PI);
158  ss << "-pointXY";
159  }
160 
161  if (hasPointBearingSensor) {
162  SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
163  robot.addSensor(bearingSensor);
164  bearingSensor->setInformation(bearingSensor->information()*1000);
165  ss << "-pointBearing";
166  }
167 
168  if (hasSegmentSensor) {
169  cerr << "creating Segment Sensor" << endl;
170  SensorSegment2D* segmentSensor = new SensorSegment2D("segmentSensor");
171  cerr << "segmentSensorCreated" << endl;
172  segmentSensor->setMaxRange(3);
173  segmentSensor->setMinRange(.1);
174  robot.addSensor(segmentSensor);
175  segmentSensor->setInformation(segmentSensor->information()*1000);
176 
177  SensorSegment2DLine* segmentSensorLine = new SensorSegment2DLine("segmentSensorSensorLine");
178  segmentSensorLine->setMaxRange(3);
179  segmentSensorLine->setMinRange(.1);
180  robot.addSensor(segmentSensorLine);
181  Matrix2d m=segmentSensorLine->information();
182  m=m*1000;
183  m(0,0)*=10;
184  segmentSensorLine->setInformation(m);
185 
186  SensorSegment2DPointLine* segmentSensorPointLine = new SensorSegment2DPointLine("segmentSensorSensorPointLine");
187  segmentSensorPointLine->setMaxRange(3);
188  segmentSensorPointLine->setMinRange(.1);
189  robot.addSensor(segmentSensorPointLine);
190  Matrix3d m3=segmentSensorPointLine->information();
191  m3=m3*1000;
192  m3(3,3)*=10;
193  segmentSensorPointLine->setInformation(m3);
194 
195  ss << "-segment2d";
196  }
197 
198 
199  robot.move(SE2());
200  double pStraight=0.7;
201  SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
202  double pLeft=0.15;
203  SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
204  //double pRight=0.15;
205  SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));
206 
207  for (int i=0; i<simSteps; i++){
208  cerr << "m";
209  SE2 move;
210  SE2 pose=robot.pose();
211  double dtheta=-100;
212  Vector2d dt;
213  if (pose.translation().x() < -.5*worldSize){
214  dtheta = 0;
215  }
216 
217  if (pose.translation().x() > .5*worldSize){
218  dtheta = -M_PI;
219  }
220 
221  if (pose.translation().y() < -.5*worldSize){
222  dtheta = M_PI/2;
223  }
224 
225  if (pose.translation().y() > .5*worldSize){
226  dtheta = -M_PI/2;
227  }
228  if (dtheta< -M_PI) {
229  // select a random move of the robot
230  double sampled=sampleUniform(0.,1.,&generator);
231  if (sampled<pStraight)
232  move=moveStraight;
233  else if (sampled<pStraight+pLeft)
234  move=moveLeft;
235  else
236  move=moveRight;
237  } else {
238  double mTheta=dtheta-pose.rotation().angle();
239  move.setRotation(Rotation2Dd(mTheta));
240  if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){
241  move.setTranslation(Vector2d(1., 0.));
242  }
243  }
244  robot.relativeMove(move);
245  // do a sense
246  cerr << "s";
247  robot.sense();
248  }
249  //string fname=outputFilename + ss.str() + ".g2o";
250  ofstream testStream(outputFilename.c_str());
251  graph.save(testStream);
252 
253  return 0;
254 }
void setMinRange(double minRange_)
Command line parsing of argc and argv.
Definition: command_args.h:46
WorldObject< VertexSegment2D > WorldObjectSegment2D
bool addSensor(BaseSensor *sensor)
Definition: simulator.cpp:51
virtual void sense()
Definition: simulator.cpp:61
represent SE2
Definition: se2.h:40
bool addRobot(BaseRobot *robot)
Definition: simulator.cpp:82
int main(int argc, char **argv)
WorldObject< VertexPointXY > WorldObjectPointXY
void setMaxRange(double maxRange_)
bool parseArgs(int argc, char **argv, bool exitOnError=true)
virtual bool save(std::ostream &os, int level=0) const
save the graph to a stream. Again uses the Factory system.
const Eigen::Rotation2Dd & rotation() const
rotational component
Definition: se2.h:58
const InformationType & information()
Definition: simulator.h:226
void paramLeftOver(const std::string &name, std::string &p, const std::string &defValue, const std::string &desc, bool optional=false)
void param(const std::string &name, bool &p, bool defValue, const std::string &desc)
const PoseType & pose() const
Definition: simulator.h:120
virtual void relativeMove(const PoseType &movement_)
Definition: simulator.h:104
bool addWorldObject(BaseWorldObject *worldObject)
Definition: simulator.cpp:90
const Vector2D & translation() const
translational component
Definition: se2.h:54
VertexType * vertex()
Definition: simulator.h:72
virtual void move(const PoseType &pose_)
Definition: simulator.h:109
#define M_PI
Definition: misc.h:34
void setInformation(const InformationType &information_)
Definition: simulator.h:221
Protocol The SLAM executable accepts such as solving the and retrieving or vertices in the graph
Definition: protocol.txt:7
void setTranslation(const Vector2D &t_)
Definition: se2.h:55
void setRotation(const Eigen::Rotation2Dd &R_)
Definition: se2.h:59
double sampleUniform(double min, double max, std::mt19937 *generator)
Definition: sampler.cpp:35