MRPT  2.0.1
CReactiveNavigationSystem3D.h
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 #pragma once
10 
12 
13 namespace mrpt::nav
14 {
15 /** A 3D robot shape stored as a "sliced" stack of 2D polygons, used for
16  * CReactiveNavigationSystem3D
17  * Depending on each PTG, only the 2D polygon or the circular radius will be
18  * taken into account
19  * \ingroup nav_reactive
20  */
22 {
23  size_t size() const { return polygons.size(); }
24  void resize(size_t num_levels)
25  {
26  polygons.resize(num_levels);
27  radius.resize(num_levels);
28  heights.resize(num_levels);
29  }
30 
31  const mrpt::math::CPolygon& polygon(size_t level) const
32  {
33  return polygons[level];
34  }
35  double getRadius(size_t level) const { return radius[level]; }
36  double getHeight(size_t level) const { return heights[level]; }
37  mrpt::math::CPolygon& polygon(size_t level) { return polygons[level]; }
38  void setRadius(size_t level, double r) { radius[level] = r; }
39  void setHeight(size_t level, double h) { heights[level] = h; }
40  const std::vector<double>& getHeights() const { return heights; }
41 
42  private:
43  std::vector<mrpt::math::CPolygon> polygons; // The polygonal bases
44  std::vector<double> radius; // The radius of each prism.
45  std::vector<double> heights; // Heights of the prisms
46 };
47 
48 /** See base class CAbstractPTGBasedReactive for a description and instructions
49  * of use.
50  * This particular implementation assumes a 3D (or "2.5D") robot shape model,
51  * build as a vertical stack of "2D slices".
52  *
53  * Paper describing the method:
54  * - M. Jaimez-Tarifa, J. Gonzalez-Jimenez, J.L. Blanco,
55  * "Efficient Reactive Navigation with Exact Collision Determination for 3D
56  * Robot Shapes",
57  * International Journal of Advanced Robotic Systems, 2015.
58  *
59  * Class history:
60  * - SEP/2012: First design.
61  * - JUL/2013: Integrated into MRPT library.
62  * - DEC/2013: Code refactoring between this class and
63  * CAbstractHolonomicReactiveMethod
64  * - FEB/2017: Refactoring of all parameters for a consistent organization in
65  * sections by class names (MRPT 1.5.0)
66  *
67  * This class requires a number of parameters which are usually provided via an
68  * external config (".ini") file.
69  * Alternatively, a memory-only object can be used to avoid physical files, see
70  * mrpt::config::CConfigFileMemory.
71  *
72  * A template config file can be generated at any moment by the user by calling
73  * saveConfigFile() with a default-constructed object.
74  *
75  * Next we provide a self-documented template config file; or see it online:
76  * https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini
77  * \verbinclude reactive3d_config.ini
78  *
79  * \sa CAbstractNavigator, CParameterizedTrajectoryGenerator,
80  * CAbstractHolonomicReactiveMethod
81  * \ingroup nav_reactive
82  */
84 {
85  public:
86  public:
87  /** See docs in ctor of base class */
89  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput = true,
90  bool enableLogFile = false,
91  const std::string& logFileDirectory =
92  std::string("./reactivenav.logs"));
93 
94  /** Destructor */
96 
97  /** Change the robot shape, which is taken into account for collision grid
98  * building. */
99  void changeRobotShape(TRobotShape robotShape);
100 
101  // See base class docs:
103  const mrpt::math::TPose2D& relative_robot_pose) const override;
104  size_t getPTG_count() const override
105  {
106  ASSERT_(!m_ptgmultilevel.empty());
107  return m_ptgmultilevel.size();
108  }
110  {
111  ASSERT_(!m_ptgmultilevel.empty() && !m_ptgmultilevel[i].PTGs.empty());
112  return m_ptgmultilevel[i]
113  .PTGs[0]
114  .get(); // Return for the 0'th level (ptgs
115  // are replicated at each level)
116  }
117  const CParameterizedTrajectoryGenerator* getPTG(size_t i) const override
118  {
119  ASSERT_(!m_ptgmultilevel.empty() && !m_ptgmultilevel[i].PTGs.empty());
120  return m_ptgmultilevel[i]
121  .PTGs[0]
122  .get(); // Return for the 0'th level (ptgs
123  // are replicated at each level)
124  }
125 
127  override; // See base class docs!
129  const override; // See base class docs!
130 
131  private:
132  // ------------------------------------------------------
133  // PRIVATE DEFINITIONS
134  // ------------------------------------------------------
135 
136  /** A set of PTGs of the same type, one per "height level" */
138  {
139  std::vector<CParameterizedTrajectoryGenerator::Ptr> PTGs;
142 
143  TPTGmultilevel() = default;
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  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  void loggingGetWSObstaclesAndShape(CLogFileRecord& out_log) override;
180 
181 }; // end class
182 } // namespace mrpt::nav
void setHeight(size_t level, double h)
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)
std::vector< CParameterizedTrajectoryGenerator::Ptr > PTGs
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...
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config 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...
double getHeight(size_t level) const
A set of PTGs of the same type, one per "height level".
TRobotShape m_robotShape
The robot 3D shape model.
const CParameterizedTrajectoryGenerator * getPTG(size_t i) const override
Gets the i&#39;th PTG.
size_t getPTG_count() const override
Returns the number of different PTGs that have been setup.
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:19
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.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
void setRadius(size_t level, double r)
CParameterizedTrajectoryGenerator * getPTG(size_t i) override
Gets the i&#39;th PTG.
#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...
double getRadius(size_t level) const
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]. ...
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
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.
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.
Definition: TPose2D.h:22
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...
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.
Stores a candidate movement in TP-Space-based navigation.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020