MRPT  1.9.9
CReactiveNavigationSystem.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 
14 #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 
41 /*---------------------------------------------------------------
42  changeRobotShape
43  ---------------------------------------------------------------*/
45 {
47  if (shape.verticesCount() < 3)
48  {
49  THROW_EXCEPTION("The robot shape has less than 3 vertices!!");
50  }
51  m_robotShape = shape;
52 }
54 {
56  ASSERT_(R > 0);
58 }
59 
62 {
64 
65  const std::string s = "CReactiveNavigationSystem";
67 
68  unsigned int PTG_COUNT = PTGs.size();
69  MRPT_SAVE_CONFIG_VAR_COMMENT(PTG_COUNT, "Number of PTGs");
70 }
71 
74 {
76 
77  // 1st: load my own params; at the end, call parent's overriden method:
78  const std::string sectCfg = "CReactiveNavigationSystem";
79  this->params_reactive_nav.loadFromConfigFile(c, sectCfg);
80 
81  unsigned int PTG_COUNT = c.read_int(sectCfg, "PTG_COUNT", 0, true);
82 
83  // Load robot shape: 1/2 polygon
84  // ---------------------------------------------
85  vector<float> xs, ys;
86  c.read_vector(
87  sectCfg, "RobotModel_shape2D_xs", vector<float>(0), xs, false);
88  c.read_vector(
89  sectCfg, "RobotModel_shape2D_ys", vector<float>(0), ys, false);
90  ASSERTMSG_(
91  xs.size() == ys.size(),
92  "Config parameters `RobotModel_shape2D_xs` and `RobotModel_shape2D_ys` "
93  "must have the same length!");
94  if (!xs.empty())
95  {
96  math::CPolygon shape;
97  for (size_t i = 0; i < xs.size(); i++) shape.AddVertex(xs[i], ys[i]);
98  changeRobotShape(shape);
99  }
100 
101  // Load robot shape: 2/2 circle
102  // ---------------------------------------------
103  const double robot_shape_radius =
104  c.read_double(sectCfg, "RobotModel_circular_shape_radius", .0, false);
105  ASSERT_(robot_shape_radius >= .0);
106  if (robot_shape_radius != .0)
107  {
108  changeRobotCircularShapeRadius(robot_shape_radius);
109  }
110 
111  // Load PTGs from file:
112  // ---------------------------------------------
113  // Free previous PTGs:
114  PTGs.clear();
115  PTGs.resize(PTG_COUNT);
116 
117  for (unsigned int n = 0; n < PTG_COUNT; n++)
118  {
119  // Factory:
120  const std::string sPTGName =
121  c.read_string(sectCfg, format("PTG%u_Type", n), "", true);
123  sPTGName, c, sectCfg, format("PTG%u_", n));
124  }
125 
127  c); // call parent's overriden method:
128 
129  MRPT_END
130 }
131 
132 /** \callergraph */
134 {
136  {
138 
139  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "STEP1_InitPTGs");
140 
141  for (unsigned int i = 0; i < PTGs.size(); i++)
142  {
143  PTGs[i]->deinitialize();
144 
145  logFmt(
147  "[CReactiveNavigationSystem::STEP1_InitPTGs] Initializing "
148  "PTG#%u (`%s`)...",
149  i, PTGs[i]->getDescription().c_str());
150 
151  // Polygonal robot shape?
152  {
153  auto* ptg = dynamic_cast<mrpt::nav::CPTG_RobotShape_Polygonal*>(
154  PTGs[i].get());
155  if (ptg) ptg->setRobotShape(m_robotShape);
156  }
157  // Circular robot shape?
158  {
159  auto* ptg = dynamic_cast<mrpt::nav::CPTG_RobotShape_Circular*>(
160  PTGs[i].get());
162  }
163 
164  // Init:
165  PTGs[i]->initialize(
166  format(
167  "%s/ReacNavGrid_%03u.dat.gz",
169  .c_str(),
170  i),
171  m_enableConsoleOutput /*verbose*/
172  );
173  logStr(mrpt::system::LVL_INFO, "Done!");
174  }
175  }
176 }
177 
178 /** \callergraph */
180  mrpt::system::TTimeStamp& obstacles_timestamp)
181 {
182  try
183  {
184  bool ret; // Return true on success
185  {
186  CTimeLoggerEntry tle1(m_timelogger, "navigationStep.STEP2_Sense");
187  CTimeLoggerEntry tle2(m_timlog_delays, "senseObstacles()");
188  ret = m_robot.senseObstacles(m_WS_Obstacles, obstacles_timestamp);
189  }
190 
191  // Optional filtering of obstacles:
193  if (ret && m_WS_filter)
194  {
195  m_WS_filter->filter(
196  &m_WS_Obstacles, obstacles_timestamp,
198  }
199 
200  return ret;
201  // Note: Clip obstacles by "z" axis coordinates is more efficiently done
202  // in STEP3_WSpaceToTPSpace()
203  }
204  catch (const std::exception& e)
205  {
207  "[CReactiveNavigationSystem::STEP2_Sense] Exception:" << e.what());
208  return false;
209  }
210  catch (...)
211  {
213  "[CReactiveNavigationSystem::STEP2_Sense] Unexpected exception!");
214  return false;
215  }
216 }
217 
218 /** \callergraph */
220  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
221  mrpt::nav::ClearanceDiagram& out_clearance,
222  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_,
223  const bool eval_clearance)
224 {
226  m_navProfiler, "CReactiveNavigationSystem::STEP3_WSpaceToTPSpace()");
227 
228  ASSERT_BELOW_(ptg_idx, this->getPTG_count());
229  CParameterizedTrajectoryGenerator* ptg = this->getPTG(ptg_idx);
230 
231  const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense(
232  rel_pose_PTG_origin_wrt_sense_);
233 
234  const float OBS_MAX_XY = params_abstract_ptg_navigator.ref_distance * 1.1f;
235 
236  // Merge all the (k,d) for which the robot collides with each obstacle
237  // point:
238  size_t nObs;
239  const float *xs, *ys, *zs;
240  m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
241 
242  for (size_t obs = 0; obs < nObs; obs++)
243  {
244  double ox, oy, oz = zs[obs];
245  rel_pose_PTG_origin_wrt_sense.composePoint(xs[obs], ys[obs], ox, oy);
246 
247  if (ox > -OBS_MAX_XY && ox < OBS_MAX_XY && oy > -OBS_MAX_XY &&
248  oy < OBS_MAX_XY && oz >= params_reactive_nav.min_obstacles_height &&
250  {
251  ptg->updateTPObstacle(ox, oy, out_TPObstacles);
252  if (eval_clearance)
253  {
254  ptg->updateClearance(ox, oy, out_clearance);
255  }
256  }
257  }
258 }
259 
260 /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the
261  * logging record for the current timestep
262  * \callergraph */
264  CLogFileRecord& out_log)
265 {
266  out_log.WS_Obstacles = m_WS_Obstacles;
268 
269  const size_t nVerts = m_robotShape.size();
270  out_log.robotShape_x.resize(nVerts);
271  out_log.robotShape_y.resize(nVerts);
273 
274  for (size_t i = 0; i < nVerts; i++)
275  {
276  out_log.robotShape_x[i] = m_robotShape.GetVertex_x(i);
277  out_log.robotShape_y[i] = m_robotShape.GetVertex_y(i);
278  }
279 }
280 
282  const mrpt::config::CConfigFileBase& c, const std::string& s)
283 {
286 }
287 
289  mrpt::config::CConfigFileBase& c, const std::string& s) const
290 {
292  min_obstacles_height,
293  "Minimum `z` coordinate of obstacles to be considered fo collision "
294  "checking");
296  max_obstacles_height,
297  "Maximum `z` coordinate of obstacles to be considered fo collision "
298  "checking");
299 }
300 
302 
303  = default;
304 
306  const mrpt::math::TPose2D& relative_robot_pose) const
307 {
308  ASSERT_(!PTGs.empty());
309  size_t nObs;
310  const float *xs, *ys, *zs;
311  m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
312 
313  for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */;
314  i++)
315  {
316  const auto ptg = PTGs[i];
317  ASSERT_(ptg != nullptr);
318  const double R = ptg->getMaxRobotRadius();
319  for (size_t obs = 0; obs < nObs; obs++)
320  {
321  const double gox = xs[obs], goy = ys[obs], oz = zs[obs];
324  {
325  continue;
326  }
327  mrpt::math::TPoint2D lo = relative_robot_pose.inverseComposePoint(
328  mrpt::math::TPoint2D(gox, goy));
329 
330  if (lo.x >= -R && lo.x <= R && lo.y >= -R && lo.y <= R &&
331  ptg->isPointInsideRobotShape(lo.x, lo.y))
332  {
333  return true; // collision!
334  }
335  }
336  }
337  return false; // No collision!
338 }
double GetVertex_x(size_t i) const
Methods for accessing the vertices.
Definition: CPolygon.h:34
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
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: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
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
Base class for all PTGs using a 2D circular robot shape model.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
void changeRobotCircularShapeRadius(const double R)
Defines the 2D circular robot shape radius, used for some PTGs for collision checking.
mrpt::maps::CSimplePointsMap m_WS_Obstacles_original
Obstacle points, before filtering (if filtering is enabled).
size_t getPTG_count() const override
Returns the number of different PTGs that have been setup.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
bool m_enableConsoleOutput
Enables / disables the console debug output.
std::vector< CParameterizedTrajectoryGenerator::Ptr > PTGs
The list of PTGs to use for navigation.
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...
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::CVectorFloat robotShape_x
The robot shape in WS.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
#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 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.
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a 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...
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. ...
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...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
mrpt::maps::CSimplePointsMap WS_Obstacles_original
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
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.
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.
CParameterizedTrajectoryGenerator * getPTG(size_t i) override
Gets the i&#39;th PTG.
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:216
void logStr(const VerbosityLevel level, std::string_view msg_str) const
Main method to add the specified message string to the logger.
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:85
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:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
#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.
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)
double m_robotShapeCircularRadius
Radius of the robot if approximated as a circle.
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
Definition: TPose2D.cpp:74
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
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:39
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...
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
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:28
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) override
Return false on any fatal error.
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 ...
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020