Main MRPT website > C++ reference for MRPT 1.5.7
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  if (m_WS_Obstacles_inlevels.size() != m_robotShape.size())
326  {
327  MRPT_LOG_WARN("checkCollisionWithLatestObstacles() skipped: no previous obstacles.");
328  return false;
329  }
330  if (m_ptgmultilevel.empty())
331  {
332  MRPT_LOG_WARN("checkCollisionWithLatestObstacles() skipped: no PTGs.");
333  return false;
334  }
335 
336  for (size_t idxH = 0; idxH < nSlices; ++idxH)
337  {
338  size_t nObs;
339  const float *xs, *ys, *zs;
340  m_WS_Obstacles_inlevels[idxH].getPointsBuffer(nObs, xs, ys, zs);
341 
342  for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */; i++)
343  {
344  ASSERT_EQUAL_(m_ptgmultilevel[i].PTGs.size(), nSlices);
345  const auto ptg = m_ptgmultilevel[i].PTGs[idxH];
346  ASSERT_(ptg != nullptr);
347 
348  const double R = ptg->getMaxRobotRadius();
349  for (size_t obs = 0; obs < nObs; obs++)
350  {
351  const double gox = xs[obs], goy = ys[obs];
353  relative_robot_pose.inverseComposePoint(mrpt::math::TPoint2D(gox, goy), lo);
354 
355  if (lo.x >= -R && lo.x <= R &&
356  lo.y >= -R && lo.y <= R &&
357  ptg->isPointInsideRobotShape(lo.x, lo.y)
358  )
359  {
360  return true; // collision!
361  }
362  }
363  }
364  }
365  return false; // No collision!
366 }
367 
#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...
double y
X,Y coordinates.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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.
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:45
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...
#define THROW_EXCEPTION(msg)
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
const mrpt::math::CPolygon & polygon(size_t level) const
bool m_enableConsoleOutput
Enables / disables the console debug output.
Clearance information for one particular PTG and one set of obstacles.
STL namespace.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
GLdouble s
Definition: glext.h:3602
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...
This is the base class for any user-defined PTG.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_END
#define MRPT_LOG_WARN(_STRING)
std::vector< TPTGmultilevel > m_ptgmultilevel
The set of PTG-transformations to be used: indices are [ptg_index][height_index]. ...
const GLubyte * c
Definition: glext.h:5590
#define MRPT_LOG_INFO(_STRING)
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...
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
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
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
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.
Lightweight 2D pose.
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
#define ASSERT_(f)
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
const double & getRadius(size_t level) const
GLenum GLsizei GLenum format
Definition: glext.h:3513
void inverseComposePoint(const TPoint2D g, TPoint2D &l) const
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
const double & getHeight(size_t level) const
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019