Main MRPT website > C++ reference for MRPT 1.9.9
CReactiveNavigationSystem.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-2018, 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/system/filesystem.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::system;
19 using namespace mrpt::math;
20 using namespace mrpt::nav;
21 using namespace std;
22 
23 /*---------------------------------------------------------------
24  Constructor
25  ---------------------------------------------------------------*/
26 CReactiveNavigationSystem::CReactiveNavigationSystem(
27  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
28  bool enableLogToFile, const std::string& logFileDirectory)
30  react_iterf_impl, enableConsoleOutput, enableLogToFile,
31  logFileDirectory)
32 {
33 }
34 
35 // Dtor:
37 {
38  this->preDestructor();
39 
40  // Free PTGs:
41  for (size_t i = 0; i < PTGs.size(); i++) delete PTGs[i];
42  PTGs.clear();
43 }
44 
45 /*---------------------------------------------------------------
46  changeRobotShape
47  ---------------------------------------------------------------*/
49 {
51  if (shape.verticesCount() < 3)
52  {
53  THROW_EXCEPTION("The robot shape has less than 3 vertices!!");
54  }
55  m_robotShape = shape;
56 }
58 {
60  ASSERT_(R > 0);
62 }
63 
66 {
68 
69  const std::string s = "CReactiveNavigationSystem";
71 
72  unsigned int PTG_COUNT = PTGs.size();
73  MRPT_SAVE_CONFIG_VAR_COMMENT(PTG_COUNT, "Number of PTGs");
74 }
75 
78 {
80 
81  // 1st: load my own params; at the end, call parent's overriden method:
82  const std::string sectCfg = "CReactiveNavigationSystem";
83  this->params_reactive_nav.loadFromConfigFile(c, sectCfg);
84 
85  unsigned int PTG_COUNT = c.read_int(sectCfg, "PTG_COUNT", 0, true);
86 
87  // Load robot shape: 1/2 polygon
88  // ---------------------------------------------
89  vector<float> xs, ys;
90  c.read_vector(
91  sectCfg, "RobotModel_shape2D_xs", vector<float>(0), xs, false);
92  c.read_vector(
93  sectCfg, "RobotModel_shape2D_ys", vector<float>(0), ys, false);
94  ASSERTMSG_(
95  xs.size() == ys.size(),
96  "Config parameters `RobotModel_shape2D_xs` and `RobotModel_shape2D_ys` "
97  "must have the same length!");
98  if (!xs.empty())
99  {
100  math::CPolygon shape;
101  for (size_t i = 0; i < xs.size(); i++) shape.AddVertex(xs[i], ys[i]);
102  changeRobotShape(shape);
103  }
104 
105  // Load robot shape: 2/2 circle
106  // ---------------------------------------------
107  const double robot_shape_radius =
108  c.read_double(sectCfg, "RobotModel_circular_shape_radius", .0, false);
109  ASSERT_(robot_shape_radius >= .0);
110  if (robot_shape_radius != .0)
111  {
112  changeRobotCircularShapeRadius(robot_shape_radius);
113  }
114 
115  // Load PTGs from file:
116  // ---------------------------------------------
117  // Free previous PTGs:
118  for (size_t i = 0; i < PTGs.size(); i++) delete PTGs[i];
119  PTGs.assign(PTG_COUNT, nullptr);
120 
121  for (unsigned int n = 0; n < PTG_COUNT; n++)
122  {
123  // Factory:
124  const std::string sPTGName =
125  c.read_string(sectCfg, format("PTG%u_Type", n), "", true);
127  sPTGName, c, sectCfg, format("PTG%u_", n));
128  }
129 
131  c); // call parent's overriden method:
132 
133  MRPT_END
134 }
135 
136 /** \callergraph */
138 {
140  {
142 
143  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "STEP1_InitPTGs");
144 
145  for (unsigned int i = 0; i < PTGs.size(); i++)
146  {
147  PTGs[i]->deinitialize();
148 
149  logFmt(
151  "[CReactiveNavigationSystem::STEP1_InitPTGs] Initializing "
152  "PTG#%u (`%s`)...",
153  i, PTGs[i]->getDescription().c_str());
154 
155  // Polygonal robot shape?
156  {
159  PTGs[i]);
160  if (ptg) ptg->setRobotShape(m_robotShape);
161  }
162  // Circular robot shape?
163  {
165  dynamic_cast<mrpt::nav::CPTG_RobotShape_Circular*>(PTGs[i]);
167  }
168 
169  // Init:
170  PTGs[i]->initialize(
171  format(
172  "%s/ReacNavGrid_%03u.dat.gz",
174  .c_str(),
175  i),
176  m_enableConsoleOutput /*verbose*/
177  );
178  logStr(mrpt::system::LVL_INFO, "Done!");
179  }
180  }
181 }
182 
183 /** \callergraph */
185  mrpt::system::TTimeStamp& obstacles_timestamp)
186 {
187  try
188  {
189  bool ret; // Return true on success
190  {
191  CTimeLoggerEntry tle1(m_timelogger, "navigationStep.STEP2_Sense");
192  CTimeLoggerEntry tle2(m_timlog_delays, "senseObstacles()");
193  ret = m_robot.senseObstacles(m_WS_Obstacles, obstacles_timestamp);
194  }
195 
196  // Optional filtering of obstacles:
198  if (ret && m_WS_filter)
199  {
200  m_WS_filter->filter(
201  &m_WS_Obstacles, obstacles_timestamp,
203  }
204 
205  return ret;
206  // Note: Clip obstacles by "z" axis coordinates is more efficiently done
207  // in STEP3_WSpaceToTPSpace()
208  }
209  catch (std::exception& e)
210  {
212  "[CReactiveNavigationSystem::STEP2_Sense] Exception:" << e.what());
213  return false;
214  }
215  catch (...)
216  {
218  "[CReactiveNavigationSystem::STEP2_Sense] Unexpected exception!");
219  return false;
220  }
221 }
222 
223 /** \callergraph */
225  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
226  mrpt::nav::ClearanceDiagram& out_clearance,
227  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_,
228  const bool eval_clearance)
229 {
230  CParameterizedTrajectoryGenerator* ptg = this->getPTG(ptg_idx);
231 
232  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(
233  rel_pose_PTG_origin_wrt_sense_);
234 
235  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance * 1.1f;
236 
237  // Merge all the (k,d) for which the robot collides with each obstacle
238  // point:
239  size_t nObs;
240  const float *xs, *ys, *zs;
241  m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
242 
243  for (size_t obs = 0; obs < nObs; obs++)
244  {
245  double ox, oy, oz = zs[obs];
246  rel_pose_PTG_origin_wrt_sense.composePoint(xs[obs], ys[obs], ox, oy);
247 
248  if (ox > -OBS_MAX_XY && ox < OBS_MAX_XY && oy > -OBS_MAX_XY &&
249  oy < OBS_MAX_XY && oz >= params_reactive_nav.min_obstacles_height &&
251  {
252  ptg->updateTPObstacle(ox, oy, out_TPObstacles);
253  if (eval_clearance)
254  {
255  ptg->updateClearance(ox, oy, out_clearance);
256  }
257  }
258  }
259 }
260 
261 /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the
262  * logging record for the current timestep
263  * \callergraph */
265  CLogFileRecord& out_log)
266 {
267  out_log.WS_Obstacles = m_WS_Obstacles;
269 
270  const size_t nVerts = m_robotShape.size();
271  out_log.robotShape_x.resize(nVerts);
272  out_log.robotShape_y.resize(nVerts);
274 
275  for (size_t i = 0; i < nVerts; i++)
276  {
277  out_log.robotShape_x[i] = m_robotShape.GetVertex_x(i);
278  out_log.robotShape_y[i] = m_robotShape.GetVertex_y(i);
279  }
280 }
281 
284 {
287 }
288 
291 {
293  min_obstacles_height,
294  "Minimum `z` coordinate of obstacles to be considered fo collision "
295  "checking");
297  max_obstacles_height,
298  "Maximum `z` coordinate of obstacles to be considered fo collision "
299  "checking");
300 }
301 
303  : min_obstacles_height(0.0), max_obstacles_height(10.0)
304 {
305 }
306 
308  const mrpt::math::TPose2D& relative_robot_pose) const
309 {
310  ASSERT_(!PTGs.empty());
311  size_t nObs;
312  const float *xs, *ys, *zs;
313  m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
314 
315  for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */;
316  i++)
317  {
318  const auto ptg = PTGs[i];
319  ASSERT_(ptg != nullptr);
320  const double R = ptg->getMaxRobotRadius();
321  for (size_t obs = 0; obs < nObs; obs++)
322  {
323  const double gox = xs[obs], goy = ys[obs], oz = zs[obs];
326  {
327  continue;
328  }
329  mrpt::math::TPoint2D lo = relative_robot_pose.inverseComposePoint(
330  mrpt::math::TPoint2D(gox, goy));
331 
332  if (lo.x >= -R && lo.x <= R && lo.y >= -R && lo.y <= R &&
333  ptg->isPointInsideRobotShape(lo.x, lo.y))
334  {
335  return true; // collision!
336  }
337  }
338  }
339  return false; // No collision!
340 }
double GetVertex_x(size_t i) const
Methods for accessing the vertices.
Definition: CPolygon.h:36
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
std::vector< CParameterizedTrajectoryGenerator * > PTGs
The list of PTGs to use for navigation.
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...
#define MRPT_START
Definition: exceptions.h:262
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:48
double x
X,Y coordinates.
mrpt::math::CVectorFloat robotShape_y
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
mrpt::maps::CSimplePointsMap m_WS_Obstacles
The obstacle points, as seen from the local robot frame.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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.
void changeRobotCircularShapeRadius(const double R)
Defines the 2D circular robot shape radius, used for some PTGs for collision checking.
GLenum GLsizei n
Definition: glext.h:5074
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...
mrpt::maps::CSimplePointsMap m_WS_Obstacles_original
Obstacle points, before filtering (if filtering is enabled).
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:21
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:3676
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:175
mrpt::math::CVectorFloat robotShape_x
The robot shape in WS.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
virtual 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...
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
const GLubyte * c
Definition: glext.h:6313
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i) override
Gets the i&#39;th PTG.
mrpt::maps::CSimplePointsMap WS_Obstacles_original
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &c, const std::string &s) override
This method load the options from a ".ini"-like file or memory-stored string list.
void updateClearance(const double ox, const double oy, ClearanceDiagram &cd) const
Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative to the PTG pa...
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method...
virtual void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const =0
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
void changeRobotShape(const mrpt::math::CPolygon &shape)
Defines the 2D polygonal robot shape, used for some PTGs for collision checking.
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:245
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
CRobot2NavInterface & m_robot
The navigator-robot interface.
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
Builds TP-Obstacles from Workspace obstacles for the given PTG.
#define MRPT_END
Definition: exceptions.h:266
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
#define MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR_NO_DEFAULT() for REQUIRED variables config file object named c and ...
Lightweight 2D pose.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
double m_robotShapeCircularRadius
Radius of the robot if approximated as a circle.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
virtual 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)...
double GetVertex_y(size_t i) const
Definition: CPolygon.h:41
mrpt::maps::CSimplePointsMap WS_Obstacles
The WS-Obstacles.
void logStr(const VerbosityLevel level, const std::string &msg_str) const
Main method to add the specified message string to the logger.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
Lightweight 2D point.
mrpt::math::CPolygon m_robotShape
The robot 2D shape model.
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:30
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) override
Return false on any fatal error.
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
Default: none.
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019