Main MRPT website > C++ reference for MRPT 1.5.7
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-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/system/filesystem.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 using namespace mrpt::utils;
20 using namespace mrpt::nav;
21 using namespace std;
22 
23 
24 /*---------------------------------------------------------------
25  Constructor
26  ---------------------------------------------------------------*/
27 CReactiveNavigationSystem::CReactiveNavigationSystem(
28  CRobot2NavInterface &react_iterf_impl,
29  bool enableConsoleOutput,
30  bool enableLogToFile,
31  const std::string &logFileDirectory
32 )
33  :
34  CAbstractPTGBasedReactive(react_iterf_impl,enableConsoleOutput,enableLogToFile, logFileDirectory)
35 {
36 }
37 
38 // Dtor:
40 {
41  this->preDestructor();
42 
43  // Free PTGs:
44  for (size_t i=0;i<PTGs.size();i++) delete PTGs[i];
45  PTGs.clear();
46 }
47 
48 
49 /*---------------------------------------------------------------
50  changeRobotShape
51  ---------------------------------------------------------------*/
53 {
55  if (shape.verticesCount() < 3) {
56  THROW_EXCEPTION("The robot shape has less than 3 vertices!!")
57  }
58  m_robotShape = shape;
59 }
61 {
63  ASSERT_(R>0);
65 }
66 
67 
69 {
71 
72  const std::string s = "CReactiveNavigationSystem";
74 
75  unsigned int PTG_COUNT = PTGs.size();
76  MRPT_SAVE_CONFIG_VAR_COMMENT(PTG_COUNT, "Number of PTGs");
77 }
78 
80 {
82 
83  // 1st: load my own params; at the end, call parent's overriden method:
84  const std::string sectCfg = "CReactiveNavigationSystem";
85  this->params_reactive_nav.loadFromConfigFile(c, sectCfg);
86 
87  unsigned int PTG_COUNT = c.read_int(sectCfg,"PTG_COUNT",0, true );
88 
89  // Load robot shape: 1/2 polygon
90  // ---------------------------------------------
91  vector<float> xs,ys;
92  c.read_vector(sectCfg,"RobotModel_shape2D_xs",vector<float>(0), xs, false );
93  c.read_vector(sectCfg,"RobotModel_shape2D_ys",vector<float>(0), ys, false );
94  ASSERTMSG_(xs.size()==ys.size(),"Config parameters `RobotModel_shape2D_xs` and `RobotModel_shape2D_ys` must have the same length!");
95  if (!xs.empty())
96  {
97  math::CPolygon shape;
98  for (size_t i=0;i<xs.size();i++)
99  shape.AddVertex(xs[i],ys[i]);
100  changeRobotShape( shape );
101  }
102 
103  // Load robot shape: 2/2 circle
104  // ---------------------------------------------
105  const double robot_shape_radius = c.read_double(sectCfg,"RobotModel_circular_shape_radius",.0, false );
106  ASSERT_(robot_shape_radius>=.0);
107  if (robot_shape_radius!=.0)
108  {
109  changeRobotCircularShapeRadius( robot_shape_radius );
110  }
111 
112  // Load PTGs from file:
113  // ---------------------------------------------
114  // Free previous PTGs:
115  for (size_t i=0;i<PTGs.size();i++) delete PTGs[i];
116  PTGs.assign(PTG_COUNT,NULL);
117 
118  for ( unsigned int n=0;n<PTG_COUNT;n++)
119  {
120  // Factory:
121  const std::string sPTGName = c.read_string(sectCfg,format("PTG%u_Type", n ),"", true );
122  PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(sPTGName,c,sectCfg, format("PTG%u_",n) );
123  }
124 
125 
126  CAbstractPTGBasedReactive::loadConfigFile(c); // call parent's overriden method:
127 
128  MRPT_END
129 }
130 
132 {
134  {
136 
137  mrpt::utils::CTimeLoggerEntry tle(m_timelogger,"STEP1_InitPTGs");
138 
139  for (unsigned int i=0;i<PTGs.size();i++)
140  {
141  PTGs[i]->deinitialize();
142 
143  logFmt(mrpt::utils::LVL_INFO,"[CReactiveNavigationSystem::STEP1_InitPTGs] Initializing PTG#%u (`%s`)...", i,PTGs[i]->getDescription().c_str());
144 
145  // Polygonal robot shape?
146  {
148  if (ptg)
150  }
151  // Circular robot shape?
152  {
154  if (ptg)
156  }
157 
158  // Init:
159  PTGs[i]->initialize(
160  format("%s/ReacNavGrid_%03u.dat.gz", params_abstract_ptg_navigator.ptg_cache_files_directory.c_str(), i),
161  m_enableConsoleOutput /*verbose*/
162  );
163  logStr(mrpt::utils::LVL_INFO,"Done!");
164  }
165  }
166 }
167 
169 {
170  try
171  {
172  bool ret; // Return true on success
173  {
174  CTimeLoggerEntry tle1(m_timelogger, "navigationStep.STEP2_Sense");
175  CTimeLoggerEntry tle2(m_timlog_delays, "senseObstacles()");
176  ret = m_robot.senseObstacles(m_WS_Obstacles, obstacles_timestamp);
177  }
178 
179  // Optional filtering of obstacles:
181  if (ret && m_WS_filter.get()!=NULL)
182  {
184  }
185 
186  return ret;
187  // Note: Clip obstacles by "z" axis coordinates is more efficiently done in STEP3_WSpaceToTPSpace()
188  }
189  catch (std::exception &e)
190  {
191  MRPT_LOG_ERROR_STREAM( "[CReactiveNavigationSystem::STEP2_Sense] Exception:" << e.what());
192  return false;
193  }
194  catch (...)
195  {
196  MRPT_LOG_ERROR_STREAM( "[CReactiveNavigationSystem::STEP2_Sense] Unexpected exception!");
197  return false;
198  }
199 
200 }
201 
202 void CReactiveNavigationSystem::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)
203 {
204  ASSERT_BELOW_(ptg_idx, this->getPTG_count());
205  CParameterizedTrajectoryGenerator *ptg = this->getPTG(ptg_idx);
206 
207  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(rel_pose_PTG_origin_wrt_sense_);
208 
209  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance*1.1f;
210 
211  // Merge all the (k,d) for which the robot collides with each obstacle point:
212  size_t nObs;
213  const float *xs,*ys,*zs;
214  m_WS_Obstacles.getPointsBuffer(nObs,xs,ys,zs);
215 
216  for (size_t obs=0;obs<nObs;obs++)
217  {
218  double ox,oy,oz=zs[obs];
219  rel_pose_PTG_origin_wrt_sense.composePoint(xs[obs], ys[obs], ox, oy);
220 
221  if (ox>-OBS_MAX_XY && ox<OBS_MAX_XY &&
222  oy>-OBS_MAX_XY && oy<OBS_MAX_XY &&
224  {
225  ptg->updateTPObstacle(ox, oy, out_TPObstacles);
226  if (eval_clearance) {
227  ptg->updateClearance(ox, oy, out_clearance);
228  }
229  }
230  }
231 }
232 
233 
234 /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
236 {
237  out_log.WS_Obstacles = m_WS_Obstacles;
239 
240  const size_t nVerts = m_robotShape.size();
241  out_log.robotShape_x.resize(nVerts);
242  out_log.robotShape_y.resize(nVerts);
244 
245  for (size_t i=0;i<nVerts;i++)
246  {
247  out_log.robotShape_x[i]= m_robotShape.GetVertex_x(i);
248  out_log.robotShape_y[i]= m_robotShape.GetVertex_y(i);
249  }
250 }
251 
253 {
256 }
257 
259 {
260  MRPT_SAVE_CONFIG_VAR_COMMENT(min_obstacles_height, "Minimum `z` coordinate of obstacles to be considered fo collision checking");
261  MRPT_SAVE_CONFIG_VAR_COMMENT(max_obstacles_height, "Maximum `z` coordinate of obstacles to be considered fo collision checking");
262 }
263 
265  min_obstacles_height(0.0),
266  max_obstacles_height(10.0)
267 {
268 }
269 
271 {
272  ASSERT_(!PTGs.empty());
273  size_t nObs;
274  const float *xs, *ys, *zs;
275  m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
276 
277  for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */; i++)
278  {
279  const auto ptg = PTGs[i];
280  ASSERT_(ptg!=nullptr);
281  const double R = ptg->getMaxRobotRadius();
282  for (size_t obs = 0; obs < nObs; obs++)
283  {
284  const double gox = xs[obs], goy = ys[obs], oz = zs[obs];
287  continue;
288  }
290  relative_robot_pose.inverseComposePoint(mrpt::math::TPoint2D(gox, goy), lo);
291 
292  if (lo.x>=-R && lo.x<=R &&
293  lo.y>=-R && lo.y<=R &&
294  ptg->isPointInsideRobotShape(lo.x, lo.y)
295  )
296  {
297  return true; // collision!
298  }
299  }
300  }
301  return false; // No collision!
302 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
double GetVertex_x(size_t i) const
Methods for accessing the vertices.
Definition: CPolygon.h:41
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
Loads all params from a file.
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
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...
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...
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
#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 ...
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:45
virtual size_t getPTG_count() const MRPT_OVERRIDE
Returns the number of different PTGs that have been setup.
mrpt::math::CVectorFloat robotShape_y
The robot shape in WS. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Polygonal.
mrpt::maps::CSimplePointsMap m_WS_Obstacles
The obstacle points, as seen from the local robot frame.
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.
#define THROW_EXCEPTION(msg)
#define ASSERT_BELOW_(__A, __B)
GLenum GLsizei n
Definition: glext.h:4618
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
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:25
bool m_enableConsoleOutput
Enables / disables the console debug output.
Clearance information for one particular PTG and one set of obstacles.
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
Loads all params from a file.
STL namespace.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
GLdouble s
Definition: glext.h:3602
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:178
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...
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)...
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
const GLubyte * c
Definition: glext.h:5590
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i) MRPT_OVERRIDE
Gets the i&#39;th PTG.
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
mrpt::maps::CSimplePointsMap WS_Obstacles_original
The WS-Obstacles.
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
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
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...
#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...
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.
mrpt::maps::CPointCloudFilterBasePtr m_WS_filter
Default: none.
void changeRobotShape(const mrpt::math::CPolygon &shape)
Defines the 2D polygonal robot shape, used for some PTGs for collision checking.
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.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
#define ASSERT_(f)
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.
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
double m_robotShapeCircularRadius
Radius of the robot if approximated as a circle. Only one of robot_shape or robot_shape_circular_radi...
double GetVertex_y(size_t i) const
Definition: CPolygon.h:42
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
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...
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
Lightweight 2D point.
mrpt::math::CPolygon m_robotShape
The robot 2D shape model. Only one of robot_shape or robot_shape_circular_radius will be used in each...
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:36
#define ASSERTMSG_(f, __ERROR_MSG)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
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...
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) MRPT_OVERRIDE
Return false on any fatal error.
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