Main MRPT website > C++ reference for MRPT 1.5.6
CReactiveNavigationSystem3D.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
13 #include <mrpt/poses/CPose3D.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::poses;
17 using namespace mrpt::math;
18 using namespace mrpt::utils;
19 using namespace mrpt::nav;
20 using namespace std;
21 
22 // --------- CReactiveNavigationSystem3D::TPTGmultilevel -----------------
23 // Ctor:
24 CReactiveNavigationSystem3D::TPTGmultilevel::TPTGmultilevel()
25 {
26 }
27 // Dtor: free PTG memory
28 CReactiveNavigationSystem3D::TPTGmultilevel::~TPTGmultilevel()
29 {
30  for (size_t i=0;i<PTGs.size();i++)
31  delete PTGs[i];
32  PTGs.clear();
33 }
34 
35 
36 /*---------------------------------------------------------------
37  Constructor
38  ---------------------------------------------------------------*/
40  CRobot2NavInterface &react_iterf_impl,
41  bool enableConsoleOutput,
42  bool enableLogToFile,
43  const std::string &logFileDirectory
44 )
45  :
46  CAbstractPTGBasedReactive(react_iterf_impl,enableConsoleOutput,enableLogToFile, logFileDirectory)
47 {
48 }
49 
50 // Dtor:
52 {
53  this->preDestructor();
54 
55  // Free PTGs:
56  m_ptgmultilevel.clear();
57 }
58 
59 
60 /*---------------------------------------------------------------
61  changeRobotShape
62  ---------------------------------------------------------------*/
64 {
66 
67  for (unsigned int i=0; i<robotShape.size(); i++)
68  {
69  if ( robotShape.polygon(i).verticesCount() < 3 )
70  THROW_EXCEPTION("The robot shape has less than 3 vertices!!")
71  }
72 
73  m_robotShape = robotShape;
74 }
75 
76 
78 {
79  const std::string s = "CReactiveNavigationSystem3D";
80 
81  unsigned int HEIGHT_LEVELS = m_robotShape.size();
82  MRPT_SAVE_CONFIG_VAR_COMMENT(HEIGHT_LEVELS, "Number of robot vertical sections");
83 
84  unsigned int PTG_COUNT = m_ptgmultilevel.size();
85  MRPT_SAVE_CONFIG_VAR_COMMENT(PTG_COUNT, "Number of PTGs");
86 }
87 
89 {
91 
93 
94  // 1st: load my own params; at the end, call parent's overriden method:
95  const std::string s = "CReactiveNavigationSystem3D";
96 
97  unsigned int num_levels;
98  vector <float> xaux,yaux;
99 
100  //Read config params which describe the robot shape
101  num_levels = c.read_int(s,"HEIGHT_LEVELS", 1, true);
102  m_robotShape.resize(num_levels);
103  for (unsigned int i=1;i<=num_levels;i++)
104  {
105  m_robotShape.setHeight(i-1, c.read_float(s,format("LEVEL%d_HEIGHT",i), 1.0, true) );
106  m_robotShape.setRadius(i-1, c.read_float(s,format("LEVEL%d_RADIUS",i), 0.5, false) );
107  c.read_vector(s,format("LEVEL%d_VECTORX",i), vector<float> (0), xaux, false);
108  c.read_vector(s,format("LEVEL%d_VECTORY",i), vector<float> (0), yaux, false);
109  ASSERT_(xaux.size() == yaux.size());
110  for (unsigned int j=0;j<xaux.size();j++)
111  m_robotShape.polygon(i-1).AddVertex(xaux[j], yaux[j]);
112  }
113 
114  // Load PTGs from file:
115  // ---------------------------------------------
116  // levels = m_robotShape.heights.size()
117 
118  unsigned int num_ptgs = c.read_int(s,"PTG_COUNT", 1, true);
119  m_ptgmultilevel.resize(num_ptgs);
120 
121  // Read each PTG parameters, and generate K x N collisiongrids
122  // K - Number of PTGs
123  // N - Number of height sections
124  for (unsigned int j=1; j<=num_ptgs; j++)
125  {
126  for (unsigned int i=1; i<=m_robotShape.size(); i++)
127  {
128  MRPT_LOG_INFO_FMT("[loadConfigFile] Generating PTG#%u at level %u...",j,i);
129  const std::string sPTGName = c.read_string(s,format("PTG%d_TYPE",j),"",true);
131  m_ptgmultilevel[j-1].PTGs.push_back(ptgaux);
132  }
133  }
134 
135  MRPT_LOG_DEBUG_FMT(" Robot height sections = %u\n", static_cast<unsigned int>(m_robotShape.size()) );
136 
137  CAbstractPTGBasedReactive::loadConfigFile(c); // call parent's overriden method:
138 
139  MRPT_END
140 }
141 
143 {
145  {
147 
148  mrpt::utils::CTimeLoggerEntry tle(m_timelogger, "STEP1_InitPTGs");
149 
150  for (unsigned int j=0; j<m_ptgmultilevel.size(); j++)
151  {
152  for (unsigned int i=0; i<m_robotShape.size(); i++)
153  {
154  m_ptgmultilevel[j].PTGs[i]->deinitialize();
155 
156  MRPT_LOG_INFO_FMT("[loadConfigFile] Initializing PTG#%u.%u... (`%s`)", j,i,m_ptgmultilevel[j].PTGs[i]->getDescription().c_str());
157 
158  // Polygonal robot shape?
159  {
161  if (ptg)
163  }
164  // Circular robot shape?
165  {
167  if (ptg)
169  }
170 
171  m_ptgmultilevel[j].PTGs[i]->initialize(
172  format("%s/ReacNavGrid_%03u_L%02u.dat.gz", params_abstract_ptg_navigator.ptg_cache_files_directory.c_str(), i, j),
173  m_enableConsoleOutput /*verbose*/
174  );
175  MRPT_LOG_INFO("...Done.");
176  }
177  }
178  }
179 }
180 
181 /*************************************************************************
182 
183  STEP2_SortObstacles
184 
185  Load the obstacles and sort them accorging to the height
186  sections used to model the robot.
187 
188 *************************************************************************/
190 {
191  //-------------------------------------------------------------------
192  // The user must implement its own method to load the obstacles from
193  // either sensor measurements or simulators (m_robot.senseObstacles(...))
194  // Data have to be subsequently sorted in height bands according to the
195  // height sections of the robot.
196  //-------------------------------------------------------------------
197 
198  m_timelogger.enter("navigationStep.STEP2_LoadAndSortObstacle");
199 
200  {
201  CTimeLoggerEntry tle(m_timlog_delays, "senseObstacles()");
202  if (!m_robot.senseObstacles(m_WS_Obstacles_unsorted, obstacles_timestamp))
203  return false;
204  }
205 
206  // Empty slice maps:
207  const size_t nSlices = m_robotShape.size();
208  m_WS_Obstacles_inlevels.resize(nSlices);
209  for (size_t i=0;i<nSlices;i++)
211 
212 
213  // Sort obstacles in "slices":
214  size_t nPts;
215  const float *xs,*ys,*zs;
217  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance*1.1f;
218 
219  for (size_t j=0; j<nPts; j++)
220  {
221  float h = 0;
222  for (size_t idxH=0;idxH<nSlices;++idxH)
223  {
224  if (zs[j] < 0.01)
225  break; // skip this points
226 
227  h += m_robotShape.getHeight(idxH);
228  if (zs[j] < h)
229  {
230  // Speed-up: If the obstacle is, for sure, out of the collision grid,
231  // just don't account for it, because we don't know its mapping into TP-Obstacles anyway...
232  if (xs[j]>-OBS_MAX_XY && xs[j]<OBS_MAX_XY && ys[j]>-OBS_MAX_XY && ys[j]<OBS_MAX_XY)
233  m_WS_Obstacles_inlevels[idxH].insertPoint(xs[j],ys[j],zs[j]);
234 
235  break; // stop searching for height slots.
236  }
237  }
238  }
239 
240  m_timelogger.leave("navigationStep.STEP2_LoadAndSortObstacle");
241 
242  return true;
243 }
244 
245 /*************************************************************************
246  Transform the obstacle into TP-Obstacles in TP-Spaces
247 *************************************************************************/
249  const size_t ptg_idx,
250  std::vector<double> &out_TPObstacles,
251  mrpt::nav::ClearanceDiagram &out_clearance,
252  const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense_,
253  const bool eval_clearance)
254 {
256  ASSERT_(!m_ptgmultilevel.empty() && m_ptgmultilevel.begin()->PTGs.size()==m_robotShape.size());
257 
258  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(rel_pose_PTG_origin_wrt_sense_);
259 
260  for (size_t j=0;j<m_robotShape.size();j++)
261  {
262  size_t nObs;
263  const float *xs,*ys,*zs;
264  m_WS_Obstacles_inlevels[j].getPointsBuffer(nObs,xs,ys,zs);
265 
266  for (size_t obs=0;obs<nObs;obs++)
267  {
268  double ox, oy;
269  rel_pose_PTG_origin_wrt_sense.composePoint(xs[obs], ys[obs], ox, oy);
270  m_ptgmultilevel[ptg_idx].PTGs[j]->updateTPObstacle(ox, oy, out_TPObstacles);
271  if (eval_clearance) {
272  m_ptgmultilevel[ptg_idx].PTGs[j]->updateClearance(ox, oy, out_clearance);
273  }
274  }
275  }
276 
277  // Distances in TP-Space are normalized to [0,1]
278  // They'll be normalized in the base abstract class:
279 }
280 
281 
282 /** Generates a pointcloud of obstacles to be saved in the logging record for the current timestep */
284 {
285 
286  out_log.WS_Obstacles.clear();
287  //Include the points of all levels (this could be improved depending on STEP2)
288  for (unsigned int i=0; i < m_WS_Obstacles_inlevels.size(); i++)
290 
291  //Polygons of each height level are drawn (but they are all shown connected...)
292  if (out_log.robotShape_x.size() == 0)
293  {
294  size_t nVerts = 0;
295  TPoint2D paux;
296  size_t cuenta = 0;
297  for (unsigned int i=0; i < m_robotShape.size(); i++)
298  nVerts += m_robotShape.polygon(i).size() + 1;
299  if (size_t(out_log.robotShape_x.size()) != nVerts)
300  {
301  out_log.robotShape_x.resize(nVerts);
302  out_log.robotShape_y.resize(nVerts);
303  }
304  for (unsigned int i=0; i<m_robotShape.size(); i++)
305  {
306  for (unsigned int j=0; j<m_robotShape.polygon(i).size(); j++)
307  {
308  paux = m_robotShape.polygon(i)[j];
309  out_log.robotShape_x[cuenta]= paux.x;
310  out_log.robotShape_y[cuenta]= paux.y;
311  cuenta++;
312  }
313  paux = m_robotShape.polygon(i)[0];
314  out_log.robotShape_x[cuenta]= paux.x;
315  out_log.robotShape_y[cuenta]= paux.y;
316  cuenta++;
317  }
318  }
320 }
321 
323 {
324  const size_t nSlices = m_robotShape.size();
325 
326  for (size_t idxH = 0; idxH < nSlices; ++idxH)
327  {
328  size_t nObs;
329  const float *xs, *ys, *zs;
330  m_WS_Obstacles_inlevels[idxH].getPointsBuffer(nObs, xs, ys, zs);
331 
332  for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */; i++)
333  {
334  const auto ptg = this->m_ptgmultilevel[i].PTGs[idxH];
335  ASSERT_(ptg != nullptr);
336 
337  const double R = ptg->getMaxRobotRadius();
338  for (size_t obs = 0; obs < nObs; obs++)
339  {
340  const double gox = xs[obs], goy = ys[obs];
342  relative_robot_pose.inverseComposePoint(mrpt::math::TPoint2D(gox, goy), lo);
343 
344  if (lo.x >= -R && lo.x <= R &&
345  lo.y >= -R && lo.y <= R &&
346  ptg->isPointInsideRobotShape(lo.x, lo.y)
347  )
348  {
349  return true; // collision!
350  }
351  }
352  }
353  }
354  return false; // No collision!
355 }
356 
#define ASSERT_EQUAL_(__A, __B)
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
Loads all params from a file.
void insertAnotherMap(const CPointsMap *otherMap, const mrpt::poses::CPose3D &otherPose)
Insert the contents of another map into this one with some geometric transformation, without fusing close points.
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) MRPT_OVERRIDE
Return false on any fatal error.
virtual bool senseObstacles(mrpt::maps::CSimplePointsMap &obstacles, mrpt::system::TTimeStamp &timestamp)=0
Return the current set of obstacle points, as seen from the local coordinate frame of the robot...
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
double y
X,Y coordinates.
void STEP3_WSpaceToTPSpace(const size_t ptg_idx, std::vector< double > &out_TPObstacles, mrpt::nav::ClearanceDiagram &out_clearance, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, const bool eval_clearance) MRPT_OVERRIDE
Builds TP-Obstacles from Workspace obstacles for the given PTG.
mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted
The unsorted set of obstacles from the sensors.
mrpt::math::CVectorFloat robotShape_y
The robot shape in WS. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Polygonal.
Base class for all PTGs using a 2D circular robot shape model.
A 3D robot shape stored as a "sliced" stack of 2D polygons, used for CReactiveNavigationSystem3D Depe...
const GLfloat * c
Definition: glew.h:10088
#define THROW_EXCEPTION(msg)
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
bool m_enableConsoleOutput
Enables / disables the console debug output.
Clearance information for one particular PTG and one set of obstacles.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
mrpt::utils::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
mrpt::math::CVectorFloat robotShape_x
This class allows loading and storing values and vectors of different types from a configuration text...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
GLdouble s
Definition: glew.h:1295
This is the base class for any user-defined PTG.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_END
std::vector< TPTGmultilevel > m_ptgmultilevel
The set of PTG-transformations to be used: indices are [ptg_index][height_index]. ...
#define MRPT_LOG_INFO(_STRING)
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
Base class for all PTGs using a 2D polygonal robot shape model.
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const MRPT_OVERRIDE
Checks whether the robot shape, when placed at the given pose (relative to the current pose)...
virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) MRPT_OVERRIDE
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the c...
#define MRPT_START
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method...
const mrpt::math::CPolygon & polygon(size_t level) const
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
std::vector< mrpt::maps::CSimplePointsMap > m_WS_Obstacles_inlevels
One point cloud per 2.5D robot-shape-slice, coordinates relative to the robot local frame...
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
CRobot2NavInterface & m_robot
The navigator-robot interface.
void changeRobotShape(TRobotShape robotShape)
Change the robot shape, which is taken into account for collision grid building.
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or NULL if there is no points in the map.
Definition: CPointsMap.cpp:252
Lightweight 2D pose.
const double & getRadius(size_t level) const
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
#define ASSERT_(f)
const double & getHeight(size_t level) const
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
void inverseComposePoint(const TPoint2D g, TPoint2D &l) const
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:45
mrpt::maps::CSimplePointsMap WS_Obstacles
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
Lightweight 2D point.
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:36
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
Loads all params from a file.
CReactiveNavigationSystem3D(CRobot2NavInterface &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false, const std::string &logFileDirectory=std::string("./reactivenav.logs"))
See docs in ctor of base class.
static CParameterizedTrajectoryGenerator * CreatePTG(const std::string &ptgClassName, const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection, const std::string &sKeyPrefix)
The class factory for creating a PTG from a list of parameters in a section of a given config file (p...
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
double robotShape_radius
The circular robot radius. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Circular.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018