MRPT  1.9.9
TWaypoint.cpp
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 
10 #include "nav-precomp.h" // Precomp header
11 
13 #include <mrpt/opengl/CArrow.h>
14 #include <mrpt/opengl/CDisk.h>
16 #include <limits>
17 
18 using namespace mrpt::nav;
19 using namespace std;
20 
21 // TWaypoint ==========
23  : target(INVALID_NUM, INVALID_NUM),
24  target_heading(INVALID_NUM),
25  target_frame_id("map"),
26  allowed_distance(INVALID_NUM),
27  speed_ratio(1.0)
28 
29 {
30 }
31 
33  double target_x, double target_y, double allowed_distance_,
34  bool allow_skip_, double target_heading_, double speed_ratio_)
35  : target(target_x, target_y),
36  target_heading(target_heading_),
37  target_frame_id("map"),
38  allowed_distance(allowed_distance_),
39  speed_ratio(speed_ratio_),
40  allow_skip(allow_skip_)
41 {
42 }
43 
44 bool TWaypoint::isValid() const
45 {
46  return (target.x != INVALID_NUM) && (target.y != INVALID_NUM) &&
48 }
49 
50 std::string TWaypoint::getAsText() const
51 {
52  std::string s;
53  if (target.x != INVALID_NUM && target.y != INVALID_NUM)
54  s += mrpt::format("target=(%8.03f,%8.03f) ", target.x, target.y);
55  else
56  s += "target=(**Coordinates not set!!**) ";
57 
59  s += mrpt::format("phi=%8.03f deg ", mrpt::RAD2DEG(target_heading));
60  else
61  s += " (heading: any) ";
62 
64  s += mrpt::format("allowed_dist=%8.03f ", allowed_distance);
65  else
66  s += " (**allowed_distance not set!!**) ";
67 
68  s += (allow_skip ? " allow_skip: YES" : " allow_skip: NO ");
69 
70  s += mrpt::format(" speed_ratio: %.01f", speed_ratio);
71  return s;
72 }
73 
74 // TWaypointSequence ==========
76 // Gets navigation params as a human-readable format:
77 std::string TWaypointSequence::getAsText() const
78 {
79  string s;
80  s += mrpt::format(
81  "List of %u waypoints:\n", static_cast<unsigned int>(waypoints.size()));
82  unsigned int i = 0;
83  for (const auto& wp : waypoints)
84  {
85  s += mrpt::format(" #%3u: ", i++);
86  s += wp.getAsText();
87  s += "\n";
88  }
89  return s;
90 }
91 
92 // TWaypointStatus ==========
95 {
96  TWaypoint::operator=(wp);
97  return *this;
98 }
99 std::string TWaypointStatus::getAsText() const
100 {
101  std::string s = TWaypoint::getAsText();
102  s += mrpt::format(" reached=%s", (reached ? "YES" : "NO "));
103  ;
104  return s;
105 }
106 
107 // TWaypointStatusSequence ======
109  : waypoints(),
110  timestamp_nav_started(INVALID_TIMESTAMP),
111 
112  last_robot_pose(
113  TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM,
114  TWaypoint::INVALID_NUM)
115 {
116 }
117 
119 {
120  string s;
121  s += mrpt::format(
122  "Status for %u waypoints:\n",
123  static_cast<unsigned int>(waypoints.size()));
124  unsigned int i = 0;
125  for (const auto& wp : waypoints)
126  {
127  s += mrpt::format(" #%3u: ", i++);
128  s += wp.getAsText();
129  s += "\n";
130  }
131  s += mrpt::format(
132  " final_goal_reached:%s waypoint_index_current_goal=%d\n",
134  return s;
135 }
136 
138  : color_regular(mrpt::img::TColor(0x00, 0x00, 0xff)),
139  color_current_goal(mrpt::img::TColor(0xff, 0x00, 0x20)),
140  color_reached(mrpt::img::TColor(0x00, 0x00, 0xc0, 0xd0))
141 
142 {
143 }
144 
148 {
149  obj.clear();
150  unsigned int idx = 0;
151  for (const auto& p : waypoints)
152  {
153  auto gl_pt = mrpt::opengl::CDisk::Create(
154  p.allow_skip ? params.outter_radius
155  : params.outter_radius_non_skippable,
156  p.allow_skip ? params.inner_radius
157  : params.inner_radius_non_skippable,
158  15);
159  gl_pt->setLocation(p.target.x, p.target.y, 0.01);
160  gl_pt->setColor_u8(params.color_regular);
161  if (params.show_labels)
162  {
163  gl_pt->setName(mrpt::format("WayPt #%2u", idx));
164  gl_pt->enableShowName(true);
165  }
166  obj.insert(gl_pt);
167 
168  if (p.target_heading != TWaypoint::INVALID_NUM)
169  {
171  0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
172  o->setPose(mrpt::poses::CPose3D(
173  p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
174  obj.insert(o);
175  }
176  ++idx;
177  }
178 }
179 
183 {
184  obj.clear();
185  {
186  unsigned int idx = 0;
187  for (const auto& p : waypoints)
188  {
189  const bool is_cur_goal = (int(idx) == waypoint_index_current_goal);
190 
192  p.reached ? params.outter_radius_reached
193  : (p.allow_skip ? params.outter_radius
194  : params.outter_radius_non_skippable),
195  p.reached ? params.inner_radius_reached
196  : (p.allow_skip ? params.inner_radius
197  : params.inner_radius_non_skippable),
198  15);
199  gl_pt->setLocation(p.target.x, p.target.y, 0.01);
200  if (params.show_labels)
201  {
202  gl_pt->setName(mrpt::format(
203  "WayPt #%2u Reach:%s", idx, p.reached ? "YES" : "NO"));
204  gl_pt->enableShowName(true);
205  }
206  gl_pt->setColor_u8(
207  is_cur_goal ? params.color_current_goal
208  : (p.reached ? params.color_reached
209  : params.color_regular));
210  obj.insert(gl_pt);
211 
212  if (p.target_heading != TWaypoint::INVALID_NUM)
213  {
215  0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
216  o->setPose(mrpt::poses::CPose3D(
217  p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
218  obj.insert(o);
219  }
220  ++idx;
221  }
222  }
223 }
224 
226  mrpt::config::CConfigFileBase& c, const std::string& s) const
227 {
228  const unsigned int N = waypoints.size();
229  c.write(s, "waypoint_count", N);
230 
231  const int NP = 27; // name padding
232 
233  for (unsigned int i = 0; i < N; i++)
234  {
235  const auto& wp = waypoints[i];
236 
237  c.write(
238  s, mrpt::format("wp%03u_allowed_distance", i), wp.allowed_distance,
239  NP);
240  c.write(s, mrpt::format("wp%03u_allow_skip", i), wp.allow_skip, NP);
241  c.write(s, mrpt::format("wp%03u_target_x", i), wp.target.x, NP);
242  c.write(s, mrpt::format("wp%03u_target_y", i), wp.target.y, NP);
243  c.write(
244  s, mrpt::format("wp%03u_target_frame_id", i), wp.target_frame_id,
245  NP);
246  c.write(
247  s, mrpt::format("wp%03u_target_heading", i), wp.target_heading, NP);
248  c.write(s, mrpt::format("wp%03u_speed_ratio", i), wp.speed_ratio, NP);
249  }
250 }
251 
253  const mrpt::config::CConfigFileBase& c, const std::string& s)
254 {
255  this->clear();
256 
257  const unsigned int N = c.read_int(s, "waypoint_count", 0, true);
258  waypoints.resize(N);
259 
260  for (unsigned int i = 0; i < N; i++)
261  {
262  auto& wp = waypoints[i];
263 
264  wp.allowed_distance = c.read_double(
265  s, mrpt::format("wp%03u_allowed_distance", i), 0, true);
266  wp.allow_skip =
267  c.read_bool(s, mrpt::format("wp%03u_allow_skip", i), true, true);
268  wp.target.x =
269  c.read_double(s, mrpt::format("wp%03u_target_x", i), 0, true);
270  wp.target.y =
271  c.read_double(s, mrpt::format("wp%03u_target_y", i), 0, true);
272  wp.target_frame_id = c.read_string(
273  s, mrpt::format("wp%03u_target_frame_id", i), "map", false);
274  double hd = c.read_double(
275  s, mrpt::format("wp%03u_target_heading", i),
277  wp.target_heading = (hd > 100) ? mrpt::nav::TWaypoint::INVALID_NUM : hd;
278  wp.speed_ratio =
279  c.read_double(s, mrpt::format("wp%03u_speed_ratio", i), 1.0, false);
280  }
281 }
void clear()
Clear the list of objects in the scene, deleting objects&#39; memory.
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
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:26
T x
X,Y coordinates.
Definition: TPoint2D.h:25
bool isValid() const
Check whether all the minimum mandatory fields have been filled by the user.
Definition: TWaypoint.cpp:44
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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
STL namespace.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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...
void save(mrpt::config::CConfigFileBase &c, const std::string &s) const
Saves waypoints to a config file section.
Definition: TWaypoint.cpp:225
static Ptr Create(Args &&... args)
Definition: CArrow.h:30
TWaypoint()
Ctor with default values.
Definition: TWaypoint.cpp:22
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
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
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
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
TWaypointStatus & operator=(const TWaypoint &wp)
Definition: TWaypoint.cpp:94
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TWaypointStatusSequence()
Ctor with default values.
Definition: TWaypoint.cpp:108
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
mrpt::math::TPoint2D target
[Must be set by the user] Coordinates of desired target location (world/global coordinates).
Definition: TWaypoint.h:28
constexpr double RAD2DEG(const double x)
Radians to degrees.
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
static Ptr Create(Args &&... args)
Definition: CDisk.h:32
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
void insert(const CRenderizable::Ptr &newObject)
Insert a new object to the list.
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:155
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
double speed_ratio
(Default=1.0) Desired robot speed at the target, as a ratio of the full robot speed.
Definition: TWaypoint.h:50



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