MRPT  2.0.3
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-2020, 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 }
mrpt::nav::CPTG_RobotShape_Polygonal
Base class for all PTGs using a 2D polygonal robot shape model.
Definition: CParameterizedTrajectoryGenerator.h:522
mrpt::nav::CPTG_RobotShape_Circular
Base class for all PTGs using a 2D circular robot shape model.
Definition: CParameterizedTrajectoryGenerator.h:565
mrpt::nav::CLogFileRecord
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
Definition: CLogFileRecord.h:29
nav-precomp.h
MRPT_LOG_INFO_FMT
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:463
mrpt::nav::CAbstractPTGBasedReactive::m_timelogger
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
Definition: CAbstractPTGBasedReactive.h:312
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
mrpt::maps::CPointsMap::insertAnotherMap
void insertAnotherMap(const CPointsMap *otherMap, const mrpt::poses::CPose3D &otherPose)
Insert the contents of another map into this one with some geometric transformation,...
Definition: CPointsMap.cpp:1695
mrpt::nav::CAbstractPTGBasedReactive
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
Definition: CAbstractPTGBasedReactive.h:92
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
Definition: system/COutputLogger.h:429
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::ref_distance
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
Definition: CAbstractPTGBasedReactive.h:184
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
mrpt::nav::CLogFileRecord::robotShape_x
mrpt::math::CVectorFloat robotShape_x
The robot shape in WS.
Definition: CLogFileRecord.h:117
mrpt::nav::CPTG_RobotShape_Circular::setRobotShapeRadius
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method.
Definition: CPTG_RobotShape_Circular.cpp:21
mrpt::math::TPoint2D_< double >
mrpt::math::CPolygon::AddVertex
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:28
mrpt::nav::CLogFileRecord::robotShape_radius
double robotShape_radius
The circular robot radius.
Definition: CLogFileRecord.h:120
mrpt::math::TPose2D::inverseComposePoint
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
Definition: TPose2D.cpp:74
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method.
Definition: CPTG_RobotShape_Polygonal.cpp:21
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::nav::CReactiveNavigationSystem3D::saveConfigFile
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
Definition: CReactiveNavigationSystem3D.cpp:59
mrpt::nav::CRobot2NavInterface
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Definition: CRobot2NavInterface.h:43
R
const float R
Definition: CKinematicChain.cpp:137
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::nav::TRobotShape::resize
void resize(size_t num_levels)
Definition: CReactiveNavigationSystem3D.h:24
mrpt::nav::CReactiveNavigationSystem3D::changeRobotShape
void changeRobotShape(TRobotShape robotShape)
Change the robot shape, which is taken into account for collision grid building.
Definition: CReactiveNavigationSystem3D.cpp:46
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::poses::CPose2D::composePoint
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:199
mrpt::math::CVectorDynamic::resize
void resize(std::size_t N, bool zeroNewElements=false)
Definition: CVectorDynamic.h:151
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:171
mrpt::nav::TRobotShape::size
size_t size() const
Definition: CReactiveNavigationSystem3D.h:23
mrpt::nav::CAbstractPTGBasedReactive::m_PTGsMustBeReInitialized
bool m_PTGsMustBeReInitialized
Definition: CAbstractPTGBasedReactive.h:313
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::system::CTimeLogger::leave
double leave(const std::string_view &func_name) noexcept
End of a named section.
Definition: system/CTimeLogger.h:146
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::nav::CReactiveNavigationSystem3D::loggingGetWSObstaclesAndShape
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...
Definition: CReactiveNavigationSystem3D.cpp:290
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::nav::ClearanceDiagram
Clearance information for one particular PTG and one set of obstacles.
Definition: ClearanceDiagram.h:28
mrpt::nav::TRobotShape::getHeight
double getHeight(size_t level) const
Definition: CReactiveNavigationSystem3D.h:36
mrpt::nav::CAbstractPTGBasedReactive::loadConfigFile
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
Definition: CAbstractPTGBasedReactive.cpp:1891
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::nav::TRobotShape::polygon
const mrpt::math::CPolygon & polygon(size_t level) const
Definition: CReactiveNavigationSystem3D.h:31
mrpt::nav::CAbstractNavigator::m_robot
CRobot2NavInterface & m_robot
The navigator-robot interface.
Definition: CAbstractNavigator.h:353
mrpt::math::CPolygon::verticesCount
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:46
mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: math/include/mrpt/math/bits_math.h:21
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::nav::CReactiveNavigationSystem3D::m_WS_Obstacles_inlevels
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.
Definition: CReactiveNavigationSystem3D.h:153
mrpt::system::CTimeLogger::enter
void enter(const std::string_view &func_name) noexcept
Start of a named section.
Definition: system/CTimeLogger.h:140
mrpt::system::TTimeStamp
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
mrpt::nav::CReactiveNavigationSystem3D::STEP3_WSpaceToTPSpace
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.
Definition: CReactiveNavigationSystem3D.cpp:248
mrpt::nav::CReactiveNavigationSystem3D::checkCollisionWithLatestObstacles
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),...
Definition: CReactiveNavigationSystem3D.cpp:332
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:118
mrpt::nav::CReactiveNavigationSystem3D::m_ptgmultilevel
std::vector< TPTGmultilevel > m_ptgmultilevel
The set of PTG-transformations to be used: indices are [ptg_index][height_index].
Definition: CReactiveNavigationSystem3D.h:160
mrpt::nav::CAbstractPTGBasedReactive::m_enableConsoleOutput
bool m_enableConsoleOutput
Enables / disables the console debug output.
Definition: CAbstractPTGBasedReactive.h:306
mrpt::nav::CParameterizedTrajectoryGenerator::CreatePTG
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...
Definition: CParameterizedTrajectoryGenerator_factory.cpp:22
mrpt::nav::CLogFileRecord::WS_Obstacles
mrpt::maps::CSimplePointsMap WS_Obstacles
The WS-Obstacles.
Definition: CLogFileRecord.h:93
MRPT_LOG_WARN
#define MRPT_LOG_WARN(_STRING)
Definition: system/COutputLogger.h:431
mrpt::nav::CReactiveNavigationSystem3D::m_robotShape
TRobotShape m_robotShape
The robot 3D shape model.
Definition: CReactiveNavigationSystem3D.h:156
CPose3D.h
mrpt::math::CVectorDynamic::size
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CVectorDynamic.h:141
mrpt::system::CTimeLoggerEntry
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
Definition: system/CTimeLogger.h:189
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::ptg_cache_files_directory
std::string ptg_cache_files_directory
(Default: ".")
Definition: CAbstractPTGBasedReactive.h:181
mrpt::maps::CPointsMap::getPointsBuffer
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:222
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
mrpt::nav::CReactiveNavigationSystem3D::~CReactiveNavigationSystem3D
~CReactiveNavigationSystem3D() override
Destructor.
Definition: CReactiveNavigationSystem3D.cpp:35
mrpt::nav::CLogFileRecord::robotShape_y
mrpt::math::CVectorFloat robotShape_y
Definition: CLogFileRecord.h:117
mrpt::nav::CReactiveNavigationSystem3D::implementSenseObstacles
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) override
Return false on any fatal error.
Definition: CReactiveNavigationSystem3D.cpp:187
mrpt::nav::TRobotShape::setHeight
void setHeight(size_t level, double h)
Definition: CReactiveNavigationSystem3D.h:39
mrpt::nav::CRobot2NavInterface::senseObstacles
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.
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::nav::CReactiveNavigationSystem3D::m_WS_Obstacles_unsorted
mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted
The unsorted set of obstacles from the sensors.
Definition: CReactiveNavigationSystem3D.h:150
mrpt::nav::CAbstractPTGBasedReactive::preDestructor
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
Definition: CAbstractPTGBasedReactive.cpp:70
mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
Definition: CAbstractPTGBasedReactive.h:234
CReactiveNavigationSystem3D.h
mrpt::nav::TRobotShape::getRadius
double getRadius(size_t level) const
Definition: CReactiveNavigationSystem3D.h:35
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::nav::CAbstractNavigator::m_timlog_delays
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
Definition: CAbstractNavigator.h:382
mrpt::nav::CReactiveNavigationSystem3D::STEP1_InitPTGs
void STEP1_InitPTGs() override
Definition: CReactiveNavigationSystem3D.cpp:138
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
mrpt::nav::CReactiveNavigationSystem3D::loadConfigFile
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
Definition: CReactiveNavigationSystem3D.cpp:72
mrpt::nav::TRobotShape::setRadius
void setRadius(size_t level, double r)
Definition: CReactiveNavigationSystem3D.h:38
mrpt::config::CConfigFileBase::read_vector
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 ....
Definition: config/CConfigFileBase.h:194
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system
Definition: backtrace.h:14
mrpt::nav::TRobotShape
A 3D robot shape stored as a "sliced" stack of 2D polygons, used for CReactiveNavigationSystem3D Depe...
Definition: CReactiveNavigationSystem3D.h:21



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020