MRPT  1.9.9
CReactiveNavigationSystem.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 /** \defgroup nav_reactive Reactive navigation classes
16  * \ingroup mrpt_nav_grp
17  */
18 
19 /** See base class CAbstractPTGBasedReactive for a description and instructions
20  * of use.
21  * This particular implementation assumes a 2D robot shape which can be
22  * polygonal or circular (depending on the selected PTGs).
23  *
24  * Publications:
25  * - Blanco, Jose-Luis, Javier Gonzalez, and Juan-Antonio Fernandez-Madrigal.
26  * ["Extending obstacle avoidance methods through multiple parameter-space
27  * transformations"](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.190.4672&rep=rep1&type=pdf).
28  * Autonomous Robots 24.1 (2008): 29-48.
29  *
30  * Class history:
31  * - 17/JUN/2004: First design.
32  * - 16/SEP/2004: Totally redesigned, according to document "MultiParametric
33  * Based Space Transformation for Reactive Navigation"
34  * - 29/SEP/2005: Totally rewritten again, for integration into MRPT library and
35  * according to the ICRA paper.
36  * - 17/OCT/2007: Whole code updated to accomodate to MRPT 0.5 and make it
37  * portable to Linux.
38  * - DEC/2013: Code refactoring between this class and
39  * CAbstractHolonomicReactiveMethod
40  * - FEB/2017: Refactoring of all parameters for a consistent organization in
41  * sections by class names (MRPT 1.5.0)
42  *
43  * This class requires a number of parameters which are usually provided via an
44  * external config (".ini") file.
45  * Alternatively, a memory-only object can be used to avoid physical files, see
46  * mrpt::config::CConfigFileMemory.
47  *
48  * A template config file can be generated at any moment by the user by calling
49  * saveConfigFile() with a default-constructed object.
50  *
51  * Next we provide a self-documented template config file; or see it online:
52  * https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini
53  * \verbinclude reactive2d_config.ini
54  *
55  * \sa CAbstractNavigator, CParameterizedTrajectoryGenerator,
56  * CAbstractHolonomicReactiveMethod
57  * \ingroup nav_reactive
58  */
60 {
61  public:
62  /** See docs in ctor of base class */
64  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput = true,
65  bool enableLogFile = false,
66  const std::string& logFileDirectory =
67  std::string("./reactivenav.logs"));
68 
69  /** Destructor
70  */
71  ~CReactiveNavigationSystem() override;
72 
73  /** Defines the 2D polygonal robot shape, used for some PTGs for collision
74  * checking. */
75  void changeRobotShape(const mrpt::math::CPolygon& shape);
76  /** Defines the 2D circular robot shape radius, used for some PTGs for
77  * collision checking. */
78  void changeRobotCircularShapeRadius(const double R);
79 
80  // See base class docs:
81  size_t getPTG_count() const override { return PTGs.size(); }
83  {
84  ASSERT_(i < PTGs.size());
85  return PTGs[i].get();
86  }
87  const CParameterizedTrajectoryGenerator* getPTG(size_t i) const override
88  {
89  ASSERT_(i < PTGs.size());
90  return PTGs[i].get();
91  }
93  const mrpt::math::TPose2D& relative_robot_pose) const override;
94 
96  {
97  double min_obstacles_height{0.0},
99  10.0}; // The range of "z" coordinates for obstacles
100  // to be considered
101 
102  void loadFromConfigFile(
104  const std::string& s) override;
105  void saveToConfigFile(
107  const std::string& s) const override;
109  };
110 
112 
114  override; // See base class docs!
116  const override; // See base class docs!
117 
118  private:
119  /** The list of PTGs to use for navigation */
120  std::vector<CParameterizedTrajectoryGenerator::Ptr> PTGs;
121 
122  // Steps for the reactive navigation sytem.
123  // ----------------------------------------------------------------------------
124  void STEP1_InitPTGs() override;
125 
126  // See docs in parent class
128  mrpt::system::TTimeStamp& obs_timestamp) override;
129 
130  protected:
131  /** The robot 2D shape model. Only one of `robot_shape` or
132  * `robot_shape_circular_radius` will be used in each PTG */
134  /** Radius of the robot if approximated as a circle. Only one of
135  * `robot_shape` or `robot_shape_circular_radius` will be used in each PTG
136  */
138 
139  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in
140  * the logging record for the current timestep */
141  void loggingGetWSObstaclesAndShape(CLogFileRecord& out_log) override;
142 
143  /** The obstacle points, as seen from the local robot frame. */
145  /** Obstacle points, before filtering (if filtering is enabled). */
147  // See docs in parent class
149  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
150  mrpt::nav::ClearanceDiagram& out_clearance,
151  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense,
152  const bool eval_clearance) override;
153 
154 }; // end class
155 } // namespace mrpt::nav
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::maps::CSimplePointsMap m_WS_Obstacles
The obstacle points, as seen from the local robot frame.
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.
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. ...
std::vector< CParameterizedTrajectoryGenerator::Ptr > PTGs
The list of PTGs to use for navigation.
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 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...
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 reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
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.
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.
const float R
const CParameterizedTrajectoryGenerator * getPTG(size_t i) const override
Gets the i&#39;th PTG.
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.
Lightweight 2D pose.
Definition: TPose2D.h:22
double m_robotShapeCircularRadius
Radius of the robot if approximated as a circle.
See base class CAbstractPTGBasedReactive for a description and instructions of use.
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)...
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::math::CPolygon m_robotShape
The robot 2D shape model.
CReactiveNavigationSystem(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.
bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) override
Return false on any fatal error.



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