MRPT  2.0.1
TWaypoint.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 #include <mrpt/img/TColor.h>
13 #include <mrpt/math/TPoint2D.h>
14 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/system/datetime.h>
17 #include <string>
18 #include <vector>
19 
20 namespace mrpt::nav
21 {
22 /** A single waypoint within TWaypointSequence. \ingroup nav_reactive */
23 struct TWaypoint
24 {
25  /** [Must be set by the user] Coordinates of desired target location
26  * (world/global coordinates).
27  * \sa target_heading */
29  /** [Default=any heading] Optionally, set to the desired orientation
30  * [radians]
31  * of the robot at this waypoint. Some navigator implementations may ignore
32  * this preferred heading anyway, read the docs of each implementation to
33  * find it out. */
35 
36  /** (Default="map") Frame ID in which target is given. Optional, use only
37  * for submapping applications. */
38  std::string target_frame_id;
39 
40  /** [Must be set by the user] How close should the robot get to this
41  * waypoint for it to be considered reached. */
43 
44  /** (Default=1.0) Desired robot speed at the target, as a ratio of the full
45  * robot speed. That is: speed_ratio=1 means that the user wants the robot
46  * to navigate to the target and smoothly continue to the next one when
47  * reached. speed_ratio=0 on the other hand means that the robot should
48  * approach this waypoint slowing down and end up totally stopped.
49  */
50  double speed_ratio;
51 
52  /** [Default=true] Whether it is allowed to the navigator to proceed to a
53  * more advanced waypoint
54  * in the sequence if it determines that it is easier to skip this one
55  * (e.g. it seems blocked by dynamic obstacles).
56  * This value is ignored for the last waypoint in a sequence, since it is
57  * always considered to be the
58  * ultimate goal and hence not subject to be skipped.
59  */
60  bool allow_skip{true};
61 
62  /** Check whether all the minimum mandatory fields have been filled by the
63  * user. */
64  bool isValid() const;
65  /** Ctor with default values */
66  TWaypoint();
67  TWaypoint(
68  double target_x, double target_y, double allowed_distance,
69  bool allow_skip = true, double target_heading_ = INVALID_NUM,
70  double speed_ratio_ = 1.0);
71  /** get in human-readable format */
72  std::string getAsText() const;
73 
74  /** The default value of fields (used to detect non-set values) */
75  static const int INVALID_NUM{-100000};
76 };
77 
78 /** used in getAsOpenglVisualization() */
80 {
82 
83  double outter_radius{.3}, inner_radius{.2};
86  double heading_arrow_len{1.0};
88  bool show_labels{true};
89 };
90 
91 /** The struct for requesting navigation requests for a sequence of waypoints.
92  * Used in CWaypointsNavigator::navigateWaypoints().
93  * Users can directly fill in the list of waypoints manipulating the public
94  * field `waypoints`.
95  * \ingroup nav_reactive */
97 {
98  std::vector<TWaypoint> waypoints;
99 
100  void clear() { waypoints.clear(); }
101  /** Ctor with default values */
103  /** Gets navigation params as a human-readable format */
104  std::string getAsText() const;
105  /** Renders the sequence of waypoints (previous contents of `obj` are
106  * cleared) */
111  /** Saves waypoints to a config file section */
112  void save(mrpt::config::CConfigFileBase& c, const std::string& s) const;
113  /** Loads waypoints to a config file section */
114  void load(const mrpt::config::CConfigFileBase& c, const std::string& s);
115 };
116 
117 /** A waypoint with an execution status. \ingroup nav_reactive */
118 struct TWaypointStatus : public TWaypoint
119 {
120  /** Whether this waypoint has been reached already (to within the allowed
121  * distance as per user specifications) or skipped. */
122  bool reached{false};
123  /** If `reached==true` this boolean tells whether the waypoint was
124  * physically reached (false) or marked as reached because it was skipped
125  * (true). */
126  bool skipped{false};
127  /** Timestamp of when this waypoint was reached. (Default=INVALID_TIMESTAMP
128  * means not reached so far) */
130  /** (Initialized to 0 automatically) How many times this waypoint has been
131  * seen as "reachable" before it being the current active waypoint. */
133 
134  TWaypointStatus();
135  TWaypointStatus& operator=(const TWaypoint& wp);
136  /** Gets navigation params as a human-readable format */
137  std::string getAsText() const;
138 };
139 
140 /** The struct for querying the status of waypoints navigation. Used in
141  * CWaypointsNavigator::getWaypointNavStatus().
142  * \ingroup nav_reactive */
144 {
145  /** Waypoints parameters and status (reached, skipped, etc.) */
146  std::vector<TWaypointStatus> waypoints;
147  /** Timestamp of user navigation command. */
149  /** Whether the final waypoint has been reached successfuly. */
150  bool final_goal_reached{false};
151  /** Index in `waypoints` of the waypoint the navigator is currently trying
152  * to reach.
153  * This will point to the last waypoint after navigation ends successfully.
154  * Its value is `-1` if navigation has not started yet */
156 
157  /** Robot pose at last time step (has INVALID_NUM fields upon
158  * initialization) */
160 
161  /** Ctor with default values */
163  /** Gets navigation params as a human-readable format */
164  std::string getAsText() const;
165 
166  /** Renders the sequence of waypoints (previous contents of `obj` are
167  * cleared) */
172 };
173 } // namespace mrpt::nav
bool allow_skip
[Default=true] Whether it is allowed to the navigator to proceed to a more advanced waypoint in the s...
Definition: TWaypoint.h:60
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:99
mrpt::system::TTimeStamp timestamp_reach
Timestamp of when this waypoint was reached.
Definition: TWaypoint.h:129
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:26
bool isValid() const
Check whether all the minimum mandatory fields have been filled by the user.
Definition: TWaypoint.cpp:44
TWaypointSequence()
Ctor with default values.
std::string getAsText() const
get in human-readable format
Definition: TWaypoint.cpp:50
used in getAsOpenglVisualization()
Definition: TWaypoint.h:79
void getAsOpenglVisualization(mrpt::opengl::CSetOfObjects &obj, const mrpt::nav::TWaypointsRenderingParams &params=mrpt::nav::TWaypointsRenderingParams()) const
Renders the sequence of waypoints (previous contents of obj are cleared)
Definition: TWaypoint.cpp:180
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:98
mrpt::vision::TStereoCalibParams params
A single waypoint within TWaypointSequence.
Definition: TWaypoint.h:23
The struct for requesting navigation requests for a sequence of waypoints.
Definition: TWaypoint.h:96
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
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
Definition: TWaypoint.h:122
void load(const mrpt::config::CConfigFileBase &c, const std::string &s)
Loads waypoints to a config file section.
Definition: TWaypoint.cpp:252
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::TPose2D last_robot_pose
Robot pose at last time step (has INVALID_NUM fields upon initialization)
Definition: TWaypoint.h:159
void save(mrpt::config::CConfigFileBase &c, const std::string &s) const
Saves waypoints to a config file section.
Definition: TWaypoint.cpp:225
TWaypoint()
Ctor with default values.
Definition: TWaypoint.cpp:22
The struct for querying the status of waypoints navigation.
Definition: TWaypoint.h:143
A waypoint with an execution status.
Definition: TWaypoint.h:118
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:150
void getAsOpenglVisualization(mrpt::opengl::CSetOfObjects &obj, const mrpt::nav::TWaypointsRenderingParams &params=mrpt::nav::TWaypointsRenderingParams()) const
Renders the sequence of waypoints (previous contents of obj are cleared)
Definition: TWaypoint.cpp:145
TWaypointStatus & operator=(const TWaypoint &wp)
Definition: TWaypoint.cpp:94
mrpt::img::TColor color_current_goal
Definition: TWaypoint.h:87
TWaypointStatusSequence()
Ctor with default values.
Definition: TWaypoint.cpp:108
mrpt::math::TPoint2D target
[Must be set by the user] Coordinates of desired target location (world/global coordinates).
Definition: TWaypoint.h:28
Lightweight 2D pose.
Definition: TPose2D.h:22
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:77
double target_heading
[Default=any heading] Optionally, set to the desired orientation [radians] of the robot at this waypo...
Definition: TWaypoint.h:34
double allowed_distance
[Must be set by the user] How close should the robot get to this waypoint for it to be considered rea...
Definition: TWaypoint.h:42
mrpt::system::TTimeStamp timestamp_nav_started
Timestamp of user navigation command.
Definition: TWaypoint.h:148
bool skipped
If reached==true this boolean tells whether the waypoint was physically reached (false) or marked as ...
Definition: TWaypoint.h:126
A RGB color - 8bit.
Definition: TColor.h:25
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:118
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
Definition: TWaypoint.h:146
static const int INVALID_NUM
The default value of fields (used to detect non-set values)
Definition: TWaypoint.h:75
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:155
int counter_seen_reachable
(Initialized to 0 automatically) How many times this waypoint has been seen as "reachable" before it ...
Definition: TWaypoint.h:132
double speed_ratio
(Default=1.0) Desired robot speed at the target, as a ratio of the full robot speed.
Definition: TWaypoint.h:50
std::string target_frame_id
(Default="map") Frame ID in which target is given.
Definition: TWaypoint.h:38



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