Main MRPT website > C++ reference for MRPT 1.5.7
nav/reactive/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-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 #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 CReactiveNavigationSystem3D
19  * Depending on each PTG, only the 2D polygon or the circular radius will be taken into account
20  * \ingroup nav_reactive
21  */
23  {
24  size_t size() const { return polygons.size(); }
25  void resize(size_t num_levels)
26  {
27  polygons.resize(num_levels);
28  radius.resize(num_levels);
29  heights.resize(num_levels);
30  }
31 
32  const mrpt::math::CPolygon & polygon(size_t level) const { return polygons[level]; }
33  const double & getRadius(size_t level) const { return radius[level]; }
34  const double & getHeight(size_t level) const { return heights[level]; }
35  mrpt::math::CPolygon & polygon(size_t level) { return polygons[level]; }
36  void setRadius(size_t level, double r) { radius[level]=r; }
37  void setHeight(size_t level, double h) { heights[level]=h; }
38 
39  const std::vector<double> &getHeights() const { return heights; }
40  private:
41  std::vector<mrpt::math::CPolygon> polygons; // The polygonal bases
42  std::vector<double> radius; // The radius of each prism.
43  std::vector<double> heights; // Heights of the prisms
44  };
45 
46  /** See base class CAbstractPTGBasedReactive for a description and instructions of use.
47  * This particular implementation assumes a 3D (or "2.5D") robot shape model, build as a vertical stack of "2D slices".
48  *
49  * Paper describing the method:
50  * - M. Jaimez-Tarifa, J. Gonzalez-Jimenez, J.L. Blanco,
51  * "Efficient Reactive Navigation with Exact Collision Determination for 3D Robot Shapes",
52  * International Journal of Advanced Robotic Systems, 2015.
53  *
54  * Class history:
55  * - SEP/2012: First design.
56  * - JUL/2013: Integrated into MRPT library.
57  * - DEC/2013: Code refactoring between this class and CAbstractHolonomicReactiveMethod
58  * - FEB/2017: Refactoring of all parameters for a consistent organization in sections by class names (MRPT 1.5.0)
59  *
60  * This class requires a number of parameters which are usually provided via an external config (".ini") file.
61  * Alternatively, a memory-only object can be used to avoid physical files, see mrpt::utils::CConfigFileMemory.
62  *
63  * A template config file can be generated at any moment by the user by calling saveConfigFile() with a default-constructed object.
64  *
65  * Next we provide a self-documented template config file; or see it online: https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini
66  * \verbinclude reactive3d_config.ini
67  *
68  * \sa CAbstractNavigator, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod
69  * \ingroup nav_reactive
70  */
72  {
73  public:
75  public:
76  /** See docs in ctor of base class */
78  CRobot2NavInterface &react_iterf_impl,
79  bool enableConsoleOutput = true,
80  bool enableLogFile = false,
81  const std::string &logFileDirectory = std::string("./reactivenav.logs")
82  );
83 
84  /** Destructor */
85  virtual ~CReactiveNavigationSystem3D();
86 
87  /** Change the robot shape, which is taken into account for collision grid building. */
88  void changeRobotShape( TRobotShape robotShape );
89 
90  // See base class docs:
91  virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const MRPT_OVERRIDE;
92  virtual size_t getPTG_count() const MRPT_OVERRIDE { ASSERT_(!m_ptgmultilevel.empty()); return m_ptgmultilevel.size(); }
94  ASSERT_(!m_ptgmultilevel.empty() && !m_ptgmultilevel[i].PTGs.empty())
95  return m_ptgmultilevel[i].PTGs[0]; // Return for the 0'th level (ptgs are replicated at each level)
96  }
97  virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const MRPT_OVERRIDE {
98  ASSERT_(!m_ptgmultilevel.empty() && !m_ptgmultilevel[i].PTGs.empty())
99  return m_ptgmultilevel[i].PTGs[0]; // Return for the 0'th level (ptgs are replicated at each level)
100  }
101 
102  virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE; // See base class docs!
103  virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE; // See base class docs!
104 
105  private:
106  // ------------------------------------------------------
107  // PRIVATE DEFINITIONS
108  // ------------------------------------------------------
109 
110  /** A set of PTGs of the same type, one per "height level" */
112  {
113  std::vector<CParameterizedTrajectoryGenerator*> PTGs;
116 
117  TPTGmultilevel();
118  ~TPTGmultilevel();
119  };
120 
121  // ------------------------------------------------------
122  // PRIVATE VARIABLES
123  // ------------------------------------------------------
124  mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted; //!< The unsorted set of obstacles from the sensors
125  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
126 
127 
128  /** The robot 3D shape model */
130 
131  /** The set of PTG-transformations to be used: indices are [ptg_index][height_index] */
132  std::vector <TPTGmultilevel> m_ptgmultilevel;
133 
134 
135  // Steps for the reactive navigation sytem.
136  // ----------------------------------------------------------------------------
137  virtual void STEP1_InitPTGs() MRPT_OVERRIDE;
138 
139  // See docs in parent class
140  bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) MRPT_OVERRIDE;
141 
142  // See docs in parent class
143  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;
144 
145  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
146  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) MRPT_OVERRIDE;
147 
148 
149  }; // end class
150  }
151 }
152 
153 
154 #endif
155 
156 
157 
158 
159 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted
The unsorted set of obstacles from the sensors.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
A 3D robot shape stored as a "sliced" stack of 2D polygons, used for CReactiveNavigationSystem3D Depe...
A set of PTGs of the same type, one per "height level".
const mrpt::math::CPolygon & polygon(size_t level) const
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:25
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.
STL namespace.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
This class allows loading and storing values and vectors of different types from a configuration text...
This is the base class for any user-defined PTG.
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:5590
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
GLsizei const GLchar ** string
Definition: glext.h:3919
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i) MRPT_OVERRIDE
Gets the i&#39;th PTG.
const std::vector< double > & getHeights() const
virtual const CParameterizedTrajectoryGenerator * getPTG(size_t i) const MRPT_OVERRIDE
Gets the i&#39;th PTG.
See base class CAbstractPTGBasedReactive for a description and instructions of use.
std::vector< mrpt::math::CPolygon > polygons
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
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...
virtual size_t getPTG_count() const MRPT_OVERRIDE
Returns the number of different PTGs that have been setup.
Lightweight 2D pose.
GLint level
Definition: glext.h:3545
#define ASSERT_(f)
const double & getRadius(size_t level) const
mrpt::math::CPolygon & polygon(size_t level)
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
Stores a candidate movement in TP-Space-based navigation.



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