MRPT  1.9.9
CReactiveNavigationSystem3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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::system;
19 using namespace mrpt::nav;
20 using namespace std;
21 
22 // --------- CReactiveNavigationSystem3D::TPTGmultilevel -----------------
23 // Ctor:
24 CReactiveNavigationSystem3D::TPTGmultilevel::TPTGmultilevel() = default;
25 // Dtor: free PTG memory
26 CReactiveNavigationSystem3D::TPTGmultilevel::~TPTGmultilevel()
27 {
28  for (auto& PTG : PTGs) delete PTG;
29  PTGs.clear();
30 }
31 
32 /*---------------------------------------------------------------
33  Constructor
34  ---------------------------------------------------------------*/
35 CReactiveNavigationSystem3D::CReactiveNavigationSystem3D(
36  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
37  bool enableLogToFile, const std::string& logFileDirectory)
39  react_iterf_impl, enableConsoleOutput, enableLogToFile,
40  logFileDirectory)
41 {
42 }
43 
44 // Dtor:
46 {
47  this->preDestructor();
48 
49  // Free PTGs:
50  m_ptgmultilevel.clear();
51 }
52 
53 /*---------------------------------------------------------------
54  changeRobotShape
55  ---------------------------------------------------------------*/
57 {
59 
60  for (unsigned int i = 0; i < robotShape.size(); i++)
61  {
62  if (robotShape.polygon(i).verticesCount() < 3)
63  THROW_EXCEPTION("The robot shape has less than 3 vertices!!");
64  }
65 
66  m_robotShape = robotShape;
67 }
68 
71 {
72  const std::string s = "CReactiveNavigationSystem3D";
73 
74  unsigned int HEIGHT_LEVELS = m_robotShape.size();
76  HEIGHT_LEVELS, "Number of robot vertical sections");
77 
78  unsigned int PTG_COUNT = m_ptgmultilevel.size();
79  MRPT_SAVE_CONFIG_VAR_COMMENT(PTG_COUNT, "Number of PTGs");
80 }
81 
84 {
86 
88 
89  // 1st: load my own params; at the end, call parent's overriden method:
90  const std::string s = "CReactiveNavigationSystem3D";
91 
92  unsigned int num_levels;
93  vector<float> xaux, yaux;
94 
95  // Read config params which describe the robot shape
96  num_levels = c.read_int(s, "HEIGHT_LEVELS", 1, true);
97  m_robotShape.resize(num_levels);
98  for (unsigned int i = 1; i <= num_levels; i++)
99  {
101  i - 1, c.read_float(s, format("LEVEL%d_HEIGHT", i), 1.0, true));
103  i - 1, c.read_float(s, format("LEVEL%d_RADIUS", i), 0.5, false));
104  c.read_vector(
105  s, format("LEVEL%d_VECTORX", i), vector<float>(0), xaux, false);
106  c.read_vector(
107  s, format("LEVEL%d_VECTORY", i), vector<float>(0), yaux, false);
108  ASSERT_(xaux.size() == yaux.size());
109  for (unsigned int j = 0; j < xaux.size(); j++)
110  m_robotShape.polygon(i - 1).AddVertex(xaux[j], yaux[j]);
111  }
112 
113  // Load PTGs from file:
114  // ---------------------------------------------
115  // levels = m_robotShape.heights.size()
116 
117  unsigned int num_ptgs = c.read_int(s, "PTG_COUNT", 1, true);
118  m_ptgmultilevel.resize(num_ptgs);
119 
120  // Read each PTG parameters, and generate K x N collisiongrids
121  // K - Number of PTGs
122  // N - Number of height sections
123  for (unsigned int j = 1; j <= num_ptgs; j++)
124  {
125  for (unsigned int i = 1; i <= m_robotShape.size(); i++)
126  {
128  "[loadConfigFile] Generating PTG#%u at level %u...", j, i);
129  const std::string sPTGName =
130  c.read_string(s, format("PTG%d_TYPE", j), "", true);
133  sPTGName, c, s, format("PTG%d_", j));
134  m_ptgmultilevel[j - 1].PTGs.push_back(ptgaux);
135  }
136  }
137 
139  " Robot height sections = %u\n",
140  static_cast<unsigned int>(m_robotShape.size()));
141 
143  c); // call parent's overriden method:
144 
145  MRPT_END
146 }
147 
148 /** \callergraph */
150 {
152  {
154 
155  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "STEP1_InitPTGs");
156 
157  for (unsigned int j = 0; j < m_ptgmultilevel.size(); j++)
158  {
159  for (unsigned int i = 0; i < m_robotShape.size(); i++)
160  {
161  m_ptgmultilevel[j].PTGs[i]->deinitialize();
162 
164  "[loadConfigFile] Initializing PTG#%u.%u... (`%s`)", j, i,
165  m_ptgmultilevel[j].PTGs[i]->getDescription().c_str());
166 
167  // Polygonal robot shape?
168  {
169  auto* ptg =
171  m_ptgmultilevel[j].PTGs[i]);
172  if (ptg) ptg->setRobotShape(m_robotShape.polygon(i));
173  }
174  // Circular robot shape?
175  {
176  auto* ptg =
178  m_ptgmultilevel[j].PTGs[i]);
179  if (ptg)
181  }
182 
183  m_ptgmultilevel[j].PTGs[i]->initialize(
184  format(
185  "%s/ReacNavGrid_%03u_L%02u.dat.gz",
187  .c_str(),
188  i, j),
189  m_enableConsoleOutput /*verbose*/
190  );
191  MRPT_LOG_INFO("...Done.");
192  }
193  }
194  }
195 }
196 
197 /** \callergraph */
199  mrpt::system::TTimeStamp& obstacles_timestamp)
200 {
201  //-------------------------------------------------------------------
202  // The user must implement its own method to load the obstacles from
203  // either sensor measurements or simulators (m_robot.senseObstacles(...))
204  // Data have to be subsequently sorted in height bands according to the
205  // height sections of the robot.
206  //-------------------------------------------------------------------
207 
208  m_timelogger.enter("navigationStep.STEP2_LoadAndSortObstacle");
209 
210  {
211  CTimeLoggerEntry tle(m_timlog_delays, "senseObstacles()");
212  if (!m_robot.senseObstacles(
213  m_WS_Obstacles_unsorted, obstacles_timestamp))
214  return false;
215  }
216 
217  // Empty slice maps:
218  const size_t nSlices = m_robotShape.size();
219  m_WS_Obstacles_inlevels.resize(nSlices);
220  for (size_t i = 0; i < nSlices; i++) m_WS_Obstacles_inlevels[i].clear();
221 
222  // Sort obstacles in "slices":
223  size_t nPts;
224  const float *xs, *ys, *zs;
225  m_WS_Obstacles_unsorted.getPointsBuffer(nPts, xs, ys, zs);
226  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance * 1.1f;
227 
228  for (size_t j = 0; j < nPts; j++)
229  {
230  float h = 0;
231  for (size_t idxH = 0; idxH < nSlices; ++idxH)
232  {
233  if (zs[j] < 0.01) break; // skip this points
234 
235  h += m_robotShape.getHeight(idxH);
236  if (zs[j] < h)
237  {
238  // Speed-up: If the obstacle is, for sure, out of the collision
239  // grid,
240  // just don't account for it, because we don't know its mapping
241  // into TP-Obstacles anyway...
242  if (xs[j] > -OBS_MAX_XY && xs[j] < OBS_MAX_XY &&
243  ys[j] > -OBS_MAX_XY && ys[j] < OBS_MAX_XY)
244  m_WS_Obstacles_inlevels[idxH].insertPoint(
245  xs[j], ys[j], zs[j]);
246 
247  break; // stop searching for height slots.
248  }
249  }
250  }
251 
252  m_timelogger.leave("navigationStep.STEP2_LoadAndSortObstacle");
253 
254  return true;
255 }
256 
257 /** Transform the obstacle into TP-Obstacles in TP-Spaces
258  * \callergraph */
260  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
261  mrpt::nav::ClearanceDiagram& out_clearance,
262  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_,
263  const bool eval_clearance)
264 {
266  ASSERT_(
267  !m_ptgmultilevel.empty() &&
268  m_ptgmultilevel.begin()->PTGs.size() == m_robotShape.size());
269 
270  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(
271  rel_pose_PTG_origin_wrt_sense_);
272 
273  for (size_t j = 0; j < m_robotShape.size(); j++)
274  {
275  size_t nObs;
276  const float *xs, *ys, *zs;
277  m_WS_Obstacles_inlevels[j].getPointsBuffer(nObs, xs, ys, zs);
278 
279  for (size_t obs = 0; obs < nObs; obs++)
280  {
281  double ox, oy;
282  rel_pose_PTG_origin_wrt_sense.composePoint(
283  xs[obs], ys[obs], ox, oy);
284  m_ptgmultilevel[ptg_idx].PTGs[j]->updateTPObstacle(
285  ox, oy, out_TPObstacles);
286  if (eval_clearance)
287  {
288  m_ptgmultilevel[ptg_idx].PTGs[j]->updateClearance(
289  ox, oy, out_clearance);
290  }
291  }
292  }
293 
294  // Distances in TP-Space are normalized to [0,1]
295  // They'll be normalized in the base abstract class:
296 }
297 
298 /** Generates a pointcloud of obstacles to be saved in the logging record for
299  * the current timestep
300  * \callergraph */
302  CLogFileRecord& out_log)
303 {
304  out_log.WS_Obstacles.clear();
305  // Include the points of all levels (this could be improved depending on
306  // STEP2)
307  for (auto& m_WS_Obstacles_inlevel : m_WS_Obstacles_inlevels)
309  &m_WS_Obstacles_inlevel, CPose3D(0, 0, 0));
310 
311  // Polygons of each height level are drawn (but they are all shown
312  // connected...)
313  if (out_log.robotShape_x.size() == 0)
314  {
315  size_t nVerts = 0;
316  TPoint2D paux;
317  size_t cuenta = 0;
318  for (unsigned int i = 0; i < m_robotShape.size(); i++)
319  nVerts += m_robotShape.polygon(i).size() + 1;
320  if (size_t(out_log.robotShape_x.size()) != nVerts)
321  {
322  out_log.robotShape_x.resize(nVerts);
323  out_log.robotShape_y.resize(nVerts);
324  }
325  for (unsigned int i = 0; i < m_robotShape.size(); i++)
326  {
327  for (unsigned int j = 0; j < m_robotShape.polygon(i).size(); j++)
328  {
329  paux = m_robotShape.polygon(i)[j];
330  out_log.robotShape_x[cuenta] = paux.x;
331  out_log.robotShape_y[cuenta] = paux.y;
332  cuenta++;
333  }
334  paux = m_robotShape.polygon(i)[0];
335  out_log.robotShape_x[cuenta] = paux.x;
336  out_log.robotShape_y[cuenta] = paux.y;
337  cuenta++;
338  }
339  }
341 }
342 
344  const mrpt::math::TPose2D& relative_robot_pose) const
345 {
346  const size_t nSlices = m_robotShape.size();
347 
348  for (size_t idxH = 0; idxH < nSlices; ++idxH)
349  {
350  size_t nObs;
351  const float *xs, *ys, *zs;
352  m_WS_Obstacles_inlevels[idxH].getPointsBuffer(nObs, xs, ys, zs);
353 
354  for (size_t i = 0;
355  i < 1 /* assume all PTGs share the same robot shape! */; i++)
356  {
357  const auto ptg = this->m_ptgmultilevel[i].PTGs[idxH];
358  ASSERT_(ptg != nullptr);
359 
360  const double R = ptg->getMaxRobotRadius();
361  for (size_t obs = 0; obs < nObs; obs++)
362  {
363  const double gox = xs[obs], goy = ys[obs];
365  relative_robot_pose.inverseComposePoint(
366  mrpt::math::TPoint2D(gox, goy));
367 
368  if (lo.x >= -R && lo.x <= R && lo.y >= -R && lo.y <= R &&
369  ptg->isPointInsideRobotShape(lo.x, lo.y))
370  {
371  return true; // collision!
372  }
373  }
374  }
375  }
376  return false; // No collision!
377 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
void setHeight(size_t level, double h)
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 checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const override
Checks whether the robot shape, when placed at the given pose (relative to the current pose)...
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...
void resize(size_t num_levels)
#define MRPT_START
Definition: exceptions.h:282
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:46
mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted
The unsorted set of obstacles from the sensors.
double x
X,Y coordinates.
mrpt::math::CVectorFloat robotShape_y
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
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...
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) override
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the c...
TRobotShape m_robotShape
The robot 3D shape model.
static CParameterizedTrajectoryGenerator * CreatePTG(const std::string &ptgClassName, const mrpt::config::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...
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:3682
void setRadius(size_t level, double r)
mrpt::math::CVectorFloat robotShape_x
The robot shape in WS.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:178
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
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:6406
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
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...
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) override
Return false on any fatal error.
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.
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...
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
Definition: CPointsMap.cpp:227
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
CRobot2NavInterface & m_robot
The navigator-robot interface.
void changeRobotShape(TRobotShape robotShape)
Change the robot shape, which is taken into account for collision grid building.
#define MRPT_END
Definition: exceptions.h:286
Lightweight 2D pose.
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
const double & getRadius(size_t level) const
GLenum GLsizei GLenum format
Definition: glext.h:3535
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) override
Transform the obstacle into TP-Obstacles in TP-Spaces.
mrpt::maps::CSimplePointsMap WS_Obstacles
The WS-Obstacles.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Lightweight 2D point.
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:28
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
const double & getHeight(size_t level) const
void enter(const std::string_view &func_name)
Start of a named section.
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
double leave(const std::string_view &func_name)
End of a named section.
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
#define MRPT_LOG_INFO(_STRING)
double robotShape_radius
The circular robot radius.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019