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 /*---------------------------------------------------------------
23  Constructor
24  ---------------------------------------------------------------*/
25 CReactiveNavigationSystem3D::CReactiveNavigationSystem3D(
26  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
27  bool enableLogToFile, const std::string& logFileDirectory)
29  react_iterf_impl, enableConsoleOutput, enableLogToFile,
30  logFileDirectory)
31 {
32 }
33 
34 // Dtor:
36 {
37  this->preDestructor();
38 
39  // Free PTGs:
40  m_ptgmultilevel.clear();
41 }
42 
43 /*---------------------------------------------------------------
44  changeRobotShape
45  ---------------------------------------------------------------*/
47 {
49 
50  for (unsigned int i = 0; i < robotShape.size(); i++)
51  {
52  if (robotShape.polygon(i).verticesCount() < 3)
53  THROW_EXCEPTION("The robot shape has less than 3 vertices!!");
54  }
55 
56  m_robotShape = robotShape;
57 }
58 
61 {
62  const std::string s = "CReactiveNavigationSystem3D";
63 
64  unsigned int HEIGHT_LEVELS = m_robotShape.size();
66  HEIGHT_LEVELS, "Number of robot vertical sections");
67 
68  unsigned int PTG_COUNT = m_ptgmultilevel.size();
69  MRPT_SAVE_CONFIG_VAR_COMMENT(PTG_COUNT, "Number of PTGs");
70 }
71 
74 {
76 
78 
79  // 1st: load my own params; at the end, call parent's overriden method:
80  const std::string s = "CReactiveNavigationSystem3D";
81 
82  unsigned int num_levels;
83  vector<float> xaux, yaux;
84 
85  // Read config params which describe the robot shape
86  num_levels = c.read_int(s, "HEIGHT_LEVELS", 1, true);
87  m_robotShape.resize(num_levels);
88  for (unsigned int i = 1; i <= num_levels; i++)
89  {
91  i - 1, c.read_float(s, format("LEVEL%d_HEIGHT", i), 1.0, true));
93  i - 1, c.read_float(s, format("LEVEL%d_RADIUS", i), 0.5, false));
94  c.read_vector(
95  s, format("LEVEL%d_VECTORX", i), vector<float>(0), xaux, false);
96  c.read_vector(
97  s, format("LEVEL%d_VECTORY", i), vector<float>(0), yaux, false);
98  ASSERT_(xaux.size() == yaux.size());
99  for (unsigned int j = 0; j < xaux.size(); j++)
100  m_robotShape.polygon(i - 1).AddVertex(xaux[j], yaux[j]);
101  }
102 
103  // Load PTGs from file:
104  // ---------------------------------------------
105  // levels = m_robotShape.heights.size()
106 
107  unsigned int num_ptgs = c.read_int(s, "PTG_COUNT", 1, true);
108  m_ptgmultilevel.resize(num_ptgs);
109 
110  // Read each PTG parameters, and generate K x N collisiongrids
111  // K - Number of PTGs
112  // N - Number of height sections
113  for (unsigned int j = 1; j <= num_ptgs; j++)
114  {
115  for (unsigned int i = 1; i <= m_robotShape.size(); i++)
116  {
118  "[loadConfigFile] Generating PTG#%u at level %u...", j, i);
119  const std::string sPTGName =
120  c.read_string(s, format("PTG%d_TYPE", j), "", true);
122  sPTGName, c, s, format("PTG%d_", j));
123  m_ptgmultilevel[j - 1].PTGs.push_back(ptgaux);
124  }
125  }
126 
128  " Robot height sections = %u\n",
129  static_cast<unsigned int>(m_robotShape.size()));
130 
132  c); // call parent's overriden method:
133 
134  MRPT_END
135 }
136 
137 /** \callergraph */
139 {
141  {
143 
144  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "STEP1_InitPTGs");
145 
146  for (unsigned int j = 0; j < m_ptgmultilevel.size(); j++)
147  {
148  for (unsigned int i = 0; i < m_robotShape.size(); i++)
149  {
150  m_ptgmultilevel[j].PTGs[i]->deinitialize();
151 
153  "[loadConfigFile] Initializing PTG#%u.%u... (`%s`)", j, i,
154  m_ptgmultilevel[j].PTGs[i]->getDescription().c_str());
155 
156  // Polygonal robot shape?
157  {
158  auto* ptg =
160  m_ptgmultilevel[j].PTGs[i].get());
161  if (ptg) ptg->setRobotShape(m_robotShape.polygon(i));
162  }
163  // Circular robot shape?
164  {
165  auto* ptg =
167  m_ptgmultilevel[j].PTGs[i].get());
168  if (ptg)
170  }
171 
172  m_ptgmultilevel[j].PTGs[i]->initialize(
173  format(
174  "%s/ReacNavGrid_%03u_L%02u.dat.gz",
176  .c_str(),
177  i, j),
178  m_enableConsoleOutput /*verbose*/
179  );
180  MRPT_LOG_INFO("...Done.");
181  }
182  }
183  }
184 }
185 
186 /** \callergraph */
188  mrpt::system::TTimeStamp& obstacles_timestamp)
189 {
190  //-------------------------------------------------------------------
191  // The user must implement its own method to load the obstacles from
192  // either sensor measurements or simulators (m_robot.senseObstacles(...))
193  // Data have to be subsequently sorted in height bands according to the
194  // height sections of the robot.
195  //-------------------------------------------------------------------
196 
197  m_timelogger.enter("navigationStep.STEP2_LoadAndSortObstacle");
198 
199  {
200  CTimeLoggerEntry tle(m_timlog_delays, "senseObstacles()");
201  if (!m_robot.senseObstacles(
202  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++) m_WS_Obstacles_inlevels[i].clear();
210 
211  // Sort obstacles in "slices":
212  size_t nPts;
213  const float *xs, *ys, *zs;
214  m_WS_Obstacles_unsorted.getPointsBuffer(nPts, xs, ys, zs);
215  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance * 1.1f;
216 
217  for (size_t j = 0; j < nPts; j++)
218  {
219  float h = 0;
220  for (size_t idxH = 0; idxH < nSlices; ++idxH)
221  {
222  if (zs[j] < 0.01) break; // skip this points
223 
224  h += m_robotShape.getHeight(idxH);
225  if (zs[j] < h)
226  {
227  // Speed-up: If the obstacle is, for sure, out of the collision
228  // grid,
229  // just don't account for it, because we don't know its mapping
230  // into TP-Obstacles anyway...
231  if (xs[j] > -OBS_MAX_XY && xs[j] < OBS_MAX_XY &&
232  ys[j] > -OBS_MAX_XY && ys[j] < OBS_MAX_XY)
233  m_WS_Obstacles_inlevels[idxH].insertPoint(
234  xs[j], ys[j], zs[j]);
235 
236  break; // stop searching for height slots.
237  }
238  }
239  }
240 
241  m_timelogger.leave("navigationStep.STEP2_LoadAndSortObstacle");
242 
243  return true;
244 }
245 
246 /** Transform the obstacle into TP-Obstacles in TP-Spaces
247  * \callergraph */
249  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
250  mrpt::nav::ClearanceDiagram& out_clearance,
251  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_,
252  const bool eval_clearance)
253 {
255  ASSERT_(
256  !m_ptgmultilevel.empty() &&
257  m_ptgmultilevel.begin()->PTGs.size() == m_robotShape.size());
258 
259  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(
260  rel_pose_PTG_origin_wrt_sense_);
261 
262  for (size_t j = 0; j < m_robotShape.size(); j++)
263  {
264  size_t nObs;
265  const float *xs, *ys, *zs;
266  m_WS_Obstacles_inlevels[j].getPointsBuffer(nObs, xs, ys, zs);
267 
268  for (size_t obs = 0; obs < nObs; obs++)
269  {
270  double ox, oy;
271  rel_pose_PTG_origin_wrt_sense.composePoint(
272  xs[obs], ys[obs], ox, oy);
273  m_ptgmultilevel[ptg_idx].PTGs[j]->updateTPObstacle(
274  ox, oy, out_TPObstacles);
275  if (eval_clearance)
276  {
277  m_ptgmultilevel[ptg_idx].PTGs[j]->updateClearance(
278  ox, oy, out_clearance);
279  }
280  }
281  }
282 
283  // Distances in TP-Space are normalized to [0,1]
284  // They'll be normalized in the base abstract class:
285 }
286 
287 /** Generates a pointcloud of obstacles to be saved in the logging record for
288  * the current timestep
289  * \callergraph */
291  CLogFileRecord& out_log)
292 {
293  out_log.WS_Obstacles.clear();
294  // Include the points of all levels (this could be improved depending on
295  // STEP2)
296  for (auto& m_WS_Obstacles_inlevel : m_WS_Obstacles_inlevels)
298  &m_WS_Obstacles_inlevel, CPose3D(0, 0, 0));
299 
300  // Polygons of each height level are drawn (but they are all shown
301  // connected...)
302  if (out_log.robotShape_x.size() == 0)
303  {
304  size_t nVerts = 0;
305  TPoint2D paux;
306  size_t cuenta = 0;
307  for (unsigned int i = 0; i < m_robotShape.size(); i++)
308  nVerts += m_robotShape.polygon(i).size() + 1;
309  if (size_t(out_log.robotShape_x.size()) != nVerts)
310  {
311  out_log.robotShape_x.resize(nVerts);
312  out_log.robotShape_y.resize(nVerts);
313  }
314  for (unsigned int i = 0; i < m_robotShape.size(); i++)
315  {
316  for (unsigned int j = 0; j < m_robotShape.polygon(i).size(); j++)
317  {
318  paux = m_robotShape.polygon(i)[j];
319  out_log.robotShape_x[cuenta] = paux.x;
320  out_log.robotShape_y[cuenta] = paux.y;
321  cuenta++;
322  }
323  paux = m_robotShape.polygon(i)[0];
324  out_log.robotShape_x[cuenta] = paux.x;
325  out_log.robotShape_y[cuenta] = paux.y;
326  cuenta++;
327  }
328  }
330 }
331 
333  const mrpt::math::TPose2D& relative_robot_pose) const
334 {
335  const size_t nSlices = m_robotShape.size();
336  if (m_WS_Obstacles_inlevels.size() != m_robotShape.size())
337  {
339  "checkCollisionWithLatestObstacles() skipped: no previous "
340  "obstacles.");
341  return false;
342  }
343  if (m_ptgmultilevel.empty())
344  {
345  MRPT_LOG_WARN("checkCollisionWithLatestObstacles() skipped: no PTGs.");
346  return false;
347  }
348 
349  for (size_t idxH = 0; idxH < nSlices; ++idxH)
350  {
351  size_t nObs;
352  const float *xs, *ys, *zs;
353  m_WS_Obstacles_inlevels[idxH].getPointsBuffer(nObs, xs, ys, zs);
354 
355  for (size_t i = 0;
356  i < 1 /* assume all PTGs share the same robot shape! */; i++)
357  {
358  ASSERT_EQUAL_(m_ptgmultilevel[i].PTGs.size(), nSlices);
359  const auto ptg = m_ptgmultilevel[i].PTGs[idxH];
360  ASSERT_(ptg != nullptr);
361 
362  const double R = ptg->getMaxRobotRadius();
363  for (size_t obs = 0; obs < nObs; obs++)
364  {
365  const double gox = xs[obs], goy = ys[obs];
367  relative_robot_pose.inverseComposePoint(
368  mrpt::math::TPoint2D(gox, goy));
369 
370  if (lo.x >= -R && lo.x <= R && lo.y >= -R && lo.y <= R &&
371  ptg->isPointInsideRobotShape(lo.x, lo.y))
372  {
373  return true; // collision!
374  }
375  }
376  }
377  }
378  return false; // No collision!
379 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
void setHeight(size_t level, double h)
double x
X,Y coordinates.
Definition: TPoint2D.h:23
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:241
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.
mrpt::math::CVectorFloat robotShape_y
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
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.
static CParameterizedTrajectoryGenerator::Ptr 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...
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:120
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 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:137
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:224
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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:245
Lightweight 2D pose.
Definition: TPose2D.h:22
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
#define MRPT_LOG_WARN(_STRING)
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.
void resize(std::size_t N, bool zeroNewElements=false)
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.
Definition: TPoint2D.h:31
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: d046c631e Tue Jul 23 10:47:41 2019 -0700 at mar jul 23 19:50:16 CEST 2019