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.present())
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  CParameterizedTrajectoryGenerator *ptg = this->getPTG(ptg_idx);
205 
206  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(rel_pose_PTG_origin_wrt_sense_);
207 
208  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance*1.1f;
209 
210  // Merge all the (k,d) for which the robot collides with each obstacle point:
211  size_t nObs;
212  const float *xs,*ys,*zs;
213  m_WS_Obstacles.getPointsBuffer(nObs,xs,ys,zs);
214 
215  for (size_t obs=0;obs<nObs;obs++)
216  {
217  double ox,oy,oz=zs[obs];
218  rel_pose_PTG_origin_wrt_sense.composePoint(xs[obs], ys[obs], ox, oy);
219 
220  if (ox>-OBS_MAX_XY && ox<OBS_MAX_XY &&
221  oy>-OBS_MAX_XY && oy<OBS_MAX_XY &&
223  {
224  ptg->updateTPObstacle(ox, oy, out_TPObstacles);
225  if (eval_clearance) {
226  ptg->updateClearance(ox, oy, out_clearance);
227  }
228  }
229  }
230 }
231 
232 
233 /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
235 {
236  out_log.WS_Obstacles = m_WS_Obstacles;
238 
239  const size_t nVerts = m_robotShape.size();
240  out_log.robotShape_x.resize(nVerts);
241  out_log.robotShape_y.resize(nVerts);
243 
244  for (size_t i=0;i<nVerts;i++)
245  {
246  out_log.robotShape_x[i]= m_robotShape.GetVertex_x(i);
247  out_log.robotShape_y[i]= m_robotShape.GetVertex_y(i);
248  }
249 }
250 
252 {
253  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(min_obstacles_height, double);
254  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(max_obstacles_height, double);
255 }
256 
258 {
259  MRPT_SAVE_CONFIG_VAR_COMMENT(min_obstacles_height, "Minimum `z` coordinate of obstacles to be considered fo collision checking");
260  MRPT_SAVE_CONFIG_VAR_COMMENT(max_obstacles_height, "Maximum `z` coordinate of obstacles to be considered fo collision checking");
261 }
262 
264  min_obstacles_height(0.0),
265  max_obstacles_height(10.0)
266 {
267 }
268 
270 {
271  ASSERT_(!PTGs.empty());
272  size_t nObs;
273  const float *xs, *ys, *zs;
274  m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
275 
276  for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */; i++)
277  {
278  const auto ptg = PTGs[i];
279  ASSERT_(ptg!=nullptr);
280  const double R = ptg->getMaxRobotRadius();
281  for (size_t obs = 0; obs < nObs; obs++)
282  {
283  const double gox = xs[obs], goy = ys[obs], oz = zs[obs];
286  continue;
287  }
289  relative_robot_pose.inverseComposePoint(mrpt::math::TPoint2D(gox, goy), lo);
290 
291  if (lo.x>=-R && lo.x<=R &&
292  lo.y>=-R && lo.y<=R &&
293  ptg->isPointInsideRobotShape(lo.x, lo.y)
294  )
295  {
296  return true; // collision!
297  }
298  }
299  }
300  return false; // No collision!
301 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
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
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_LOAD_CONFIG_VAR_REQUIRED_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR_NO_DEFAULT() for REQUIRED variables config file object named c and ...
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.
const GLfloat * c
Definition: glew.h:10088
#define THROW_EXCEPTION(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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).
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 ...
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...
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...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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)...
GLdouble s
Definition: glew.h:1295
This is the base class for any user-defined PTG.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
GLsizei n
Definition: glew.h:5051
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_END
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.
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
double GetVertex_x(size_t i) const
Methods for accessing the vertices.
Definition: CPolygon.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#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.
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
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
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.
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
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.
void inverseComposePoint(const TPoint2D g, TPoint2D &l) const
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...
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:45
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.
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)
double GetVertex_y(size_t i) const
Definition: CPolygon.h:42
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.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018