Main MRPT website > C++ reference for MRPT 1.9.9
CReactiveNavigationSystem3D.h
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 #ifndef CReactiveNavigationSystem3D_H
10 #define CReactiveNavigationSystem3D_H
11 
13 
14 namespace mrpt
15 {
16 namespace nav
17 {
18 /** A 3D robot shape stored as a "sliced" stack of 2D polygons, used for
19  * CReactiveNavigationSystem3D
20  * Depending on each PTG, only the 2D polygon or the circular radius will be
21  * taken into account
22  * \ingroup nav_reactive
23  */
25 {
26  size_t size() const { return polygons.size(); }
27  void resize(size_t num_levels)
28  {
29  polygons.resize(num_levels);
30  radius.resize(num_levels);
31  heights.resize(num_levels);
32  }
33 
34  const mrpt::math::CPolygon& polygon(size_t level) const
35  {
36  return polygons[level];
37  }
38  const double& getRadius(size_t level) const { return radius[level]; }
39  const double& getHeight(size_t level) const { return heights[level]; }
41  void setRadius(size_t level, double r) { radius[level] = r; }
42  void setHeight(size_t level, double h) { heights[level] = h; }
43  const std::vector<double>& getHeights() const { return heights; }
44  private:
45  std::vector<mrpt::math::CPolygon> polygons; // The polygonal bases
46  std::vector<double> radius; // The radius of each prism.
47  std::vector<double> heights; // Heights of the prisms
48 };
49 
50 /** See base class CAbstractPTGBasedReactive for a description and instructions
51 * of use.
52 * This particular implementation assumes a 3D (or "2.5D") robot shape model,
53 * build as a vertical stack of "2D slices".
54 *
55 * Paper describing the method:
56 * - M. Jaimez-Tarifa, J. Gonzalez-Jimenez, J.L. Blanco,
57 * "Efficient Reactive Navigation with Exact Collision Determination for 3D
58 * Robot Shapes",
59 * International Journal of Advanced Robotic Systems, 2015.
60 *
61 * Class history:
62 * - SEP/2012: First design.
63 * - JUL/2013: Integrated into MRPT library.
64 * - DEC/2013: Code refactoring between this class and
65 * CAbstractHolonomicReactiveMethod
66 * - FEB/2017: Refactoring of all parameters for a consistent organization in
67 * sections by class names (MRPT 1.5.0)
68 *
69 * This class requires a number of parameters which are usually provided via an
70 * external config (".ini") file.
71 * Alternatively, a memory-only object can be used to avoid physical files, see
72 * mrpt::config::CConfigFileMemory.
73 *
74 * A template config file can be generated at any moment by the user by calling
75 * saveConfigFile() with a default-constructed object.
76 *
77 * Next we provide a self-documented template config file; or see it online:
78 * https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini
79 * \verbinclude reactive3d_config.ini
80 *
81 * \sa CAbstractNavigator, CParameterizedTrajectoryGenerator,
82 * CAbstractHolonomicReactiveMethod
83 * \ingroup nav_reactive
84 */
86 {
87  public:
89  public:
90  /** See docs in ctor of base class */
92  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput = true,
93  bool enableLogFile = false, const std::string& logFileDirectory =
94  std::string("./reactivenav.logs"));
95 
96  /** Destructor */
98 
99  /** Change the robot shape, which is taken into account for collision grid
100  * building. */
101  void changeRobotShape(TRobotShape robotShape);
102 
103  // See base class docs:
105  const mrpt::math::TPose2D& relative_robot_pose) const override;
106  virtual size_t getPTG_count() const override
107  {
108  ASSERT_(!m_ptgmultilevel.empty());
109  return m_ptgmultilevel.size();
110  }
111  virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) override
112  {
113  ASSERT_(!m_ptgmultilevel.empty() && !m_ptgmultilevel[i].PTGs.empty());
114  return m_ptgmultilevel[i].PTGs[0]; // Return for the 0'th level (ptgs
115  // are replicated at each level)
116  }
118  size_t i) const override
119  {
120  ASSERT_(!m_ptgmultilevel.empty() && !m_ptgmultilevel[i].PTGs.empty());
121  return m_ptgmultilevel[i].PTGs[0]; // Return for the 0'th level (ptgs
122  // are replicated at each level)
123  }
124 
125  virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c)
126  override; // See base class docs!
128  const override; // See base class docs!
129 
130  private:
131  // ------------------------------------------------------
132  // PRIVATE DEFINITIONS
133  // ------------------------------------------------------
134 
135  /** A set of PTGs of the same type, one per "height level" */
137  {
138  std::vector<CParameterizedTrajectoryGenerator*> PTGs;
141 
142  TPTGmultilevel();
143  ~TPTGmultilevel();
144  };
145 
146  // ------------------------------------------------------
147  // PRIVATE VARIABLES
148  // ------------------------------------------------------
149  /** The unsorted set of obstacles from the sensors */
151  /** One point cloud per 2.5D robot-shape-slice, coordinates relative to the
152  * robot local frame */
153  std::vector<mrpt::maps::CSimplePointsMap> m_WS_Obstacles_inlevels;
154 
155  /** The robot 3D shape model */
157 
158  /** The set of PTG-transformations to be used: indices are
159  * [ptg_index][height_index] */
160  std::vector<TPTGmultilevel> m_ptgmultilevel;
161 
162  // Steps for the reactive navigation sytem.
163  // ----------------------------------------------------------------------------
164  virtual void STEP1_InitPTGs() override;
165 
166  // See docs in parent class
168  mrpt::system::TTimeStamp& obs_timestamp) override;
169 
170  // See docs in parent class
172  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
173  mrpt::nav::ClearanceDiagram& out_clearance,
174  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense,
175  const bool eval_clearance) override;
176 
177  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in
178  * the logging record for the current timestep */
179  virtual void loggingGetWSObstaclesAndShape(
180  CLogFileRecord& out_log) override;
181 
182 }; // end class
183 }
184 }
185 
186 #endif
void setHeight(size_t level, double h)
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)...
void resize(size_t num_levels)
mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted
The unsorted set of obstacles from the sensors.
A 3D robot shape stored as a "sliced" stack of 2D polygons, used for CReactiveNavigationSystem3D Depe...
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config 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...
A set of PTGs of the same type, one per "height level".
TRobotShape m_robotShape
The robot 3D shape model.
std::vector< CParameterizedTrajectoryGenerator * > PTGs
const mrpt::math::CPolygon & polygon(size_t level) const
void enableLogFile(bool enable)
Enables/disables saving log files.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:21
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
Clearance information for one particular PTG and one set of obstacles.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Put this macro inside any class with members that require {16,32,64}-byte memory alignment (e...
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
void setRadius(size_t level, double r)
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i) override
Gets the i&#39;th PTG.
#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...
virtual const CParameterizedTrajectoryGenerator * getPTG(size_t i) const override
Gets the i&#39;th PTG.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
std::vector< TPTGmultilevel > m_ptgmultilevel
The set of PTG-transformations to be used: indices are [ptg_index][height_index]. ...
const GLubyte * c
Definition: glext.h:6313
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
GLsizei const GLchar ** string
Definition: glext.h:4101
const std::vector< double > & getHeights() const
See base class CAbstractPTGBasedReactive for a description and instructions of use.
std::vector< mrpt::math::CPolygon > polygons
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) override
Return false on any fatal error.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
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...
void changeRobotShape(TRobotShape robotShape)
Change the robot shape, which is taken into account for collision grid building.
Lightweight 2D pose.
GLint level
Definition: glext.h:3600
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
const double & getRadius(size_t level) const
mrpt::math::CPolygon & polygon(size_t level)
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.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Lightweight 2D point.
const double & getHeight(size_t level) const
CReactiveNavigationSystem3D(CRobot2NavInterface &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false, const std::string &logFileDirectory=std::string("./reactivenav.logs"))
See docs in ctor of base class.
virtual size_t getPTG_count() const override
Returns the number of different PTGs that have been setup.
Stores a candidate movement in TP-Space-based navigation.



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