MRPT  1.9.9
TWaypoint.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, 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 
10 #include "nav-precomp.h" // Precomp header
11 
14 #include <mrpt/opengl/CDisk.h>
15 #include <mrpt/opengl/CArrow.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  allow_skip(true)
28 {
29 }
30 
32  double target_x, double target_y, double allowed_distance_,
33  bool allow_skip_, double target_heading_)
34  : target(target_x, target_y),
35  target_heading(target_heading_),
36  target_frame_id("map"),
37  allowed_distance(allowed_distance_),
38  allow_skip(allow_skip_)
39 {
40 }
41 
42 bool TWaypoint::isValid() const
43 {
44  return (target.x != INVALID_NUM) && (target.y != INVALID_NUM) &&
46 }
47 
49 {
50  std::string s;
51  if (target.x != INVALID_NUM && target.y != INVALID_NUM)
52  s += mrpt::format("target=(%8.03f,%8.03f) ", target.x, target.y);
53  else
54  s += "target=(**Coordinates not set!!**) ";
55 
57  s += mrpt::format("phi=%8.03f deg ", mrpt::RAD2DEG(target_heading));
58  else
59  s += " (heading: any) ";
60 
62  s += mrpt::format("allowed_dist=%8.03f ", allowed_distance);
63  else
64  s += " (**allowed_distance not set!!**) ";
65 
66  s += (allow_skip ? " allow_skip: YES" : " allow_skip: NO ");
67 
68  return s;
69 }
70 
71 // TWaypointSequence ==========
73 // Gets navigation params as a human-readable format:
75 {
76  string s;
77  s += mrpt::format(
78  "List of %u waypoints:\n", static_cast<unsigned int>(waypoints.size()));
79  unsigned int i = 0;
80  for (const auto& wp : waypoints)
81  {
82  s += mrpt::format(" #%3u: ", i++);
83  s += wp.getAsText();
84  s += "\n";
85  }
86  return s;
87 }
88 
89 // TWaypointStatus ==========
91  : reached(false),
92  skipped(false),
93  timestamp_reach(INVALID_TIMESTAMP),
94  counter_seen_reachable(0)
95 {
96 }
98 {
99  TWaypoint::operator=(wp);
100  return *this;
101 }
103 {
105  s += mrpt::format(" reached=%s", (reached ? "YES" : "NO "));
106  ;
107  return s;
108 }
109 
110 // TWaypointStatusSequence ======
112  : waypoints(),
113  timestamp_nav_started(INVALID_TIMESTAMP),
114  final_goal_reached(false),
115  waypoint_index_current_goal(-1),
116  last_robot_pose(
117  TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM,
118  TWaypoint::INVALID_NUM)
119 {
120 }
121 
123 {
124  string s;
125  s += mrpt::format(
126  "Status for %u waypoints:\n",
127  static_cast<unsigned int>(waypoints.size()));
128  unsigned int i = 0;
129  for (const auto& wp : waypoints)
130  {
131  s += mrpt::format(" #%3u: ", i++);
132  s += wp.getAsText();
133  s += "\n";
134  }
135  s += mrpt::format(
136  " final_goal_reached:%s waypoint_index_current_goal=%d\n",
138  return s;
139 }
140 
142  : outter_radius(.3),
143  inner_radius(.2),
144  outter_radius_non_skippable(.3),
145  inner_radius_non_skippable(.0),
146  outter_radius_reached(.2),
147  inner_radius_reached(.1),
148  heading_arrow_len(1.0),
149  color_regular(mrpt::img::TColor(0x00, 0x00, 0xff)),
150  color_current_goal(mrpt::img::TColor(0xff, 0x00, 0x20)),
151  color_reached(mrpt::img::TColor(0x00, 0x00, 0xc0, 0xd0)),
152  show_labels(true)
153 {
154 }
155 
159 {
160  obj.clear();
161  unsigned int idx = 0;
162  for (const auto& p : waypoints)
163  {
164  auto gl_pt = mrpt::make_aligned_shared<mrpt::opengl::CDisk>(
165  p.allow_skip ? params.outter_radius
166  : params.outter_radius_non_skippable,
167  p.allow_skip ? params.inner_radius
168  : params.inner_radius_non_skippable,
169  15);
170  gl_pt->setLocation(p.target.x, p.target.y, 0.01);
171  gl_pt->setColor_u8(params.color_regular);
172  if (params.show_labels)
173  {
174  gl_pt->setName(mrpt::format("WayPt #%2u", idx));
175  gl_pt->enableShowName(true);
176  }
177  obj.insert(gl_pt);
178 
179  if (p.target_heading != TWaypoint::INVALID_NUM)
180  {
181  auto o = mrpt::make_aligned_shared<mrpt::opengl::CArrow>(
182  0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
183  o->setPose(
185  p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
186  obj.insert(o);
187  }
188  ++idx;
189  }
190 }
191 
195 {
196  obj.clear();
197  {
198  unsigned int idx = 0;
199  for (const auto& p : waypoints)
200  {
201  const bool is_cur_goal = (int(idx) == waypoint_index_current_goal);
202 
204  mrpt::make_aligned_shared<mrpt::opengl::CDisk>(
205  p.reached
206  ? params.outter_radius_reached
207  : (p.allow_skip ? params.outter_radius
208  : params.outter_radius_non_skippable),
209  p.reached
210  ? params.inner_radius_reached
211  : (p.allow_skip ? params.inner_radius
212  : params.inner_radius_non_skippable),
213  15);
214  gl_pt->setLocation(p.target.x, p.target.y, 0.01);
215  if (params.show_labels)
216  {
217  gl_pt->setName(
218  mrpt::format(
219  "WayPt #%2u Reach:%s", idx, p.reached ? "YES" : "NO"));
220  gl_pt->enableShowName(true);
221  }
222  gl_pt->setColor_u8(
223  is_cur_goal ? params.color_current_goal
224  : (p.reached ? params.color_reached
225  : params.color_regular));
226  obj.insert(gl_pt);
227 
228  if (p.target_heading != TWaypoint::INVALID_NUM)
229  {
230  auto o = mrpt::make_aligned_shared<mrpt::opengl::CArrow>(
231  0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
232  o->setPose(
234  p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
235  obj.insert(o);
236  }
237  ++idx;
238  }
239  }
240 }
241 
244 {
245  const unsigned int N = waypoints.size();
246  c.write(s, "waypoint_count", N);
247 
248  const int NP = 27; // name padding
249 
250  for (unsigned int i = 0; i < N; i++)
251  {
252  const auto& wp = waypoints[i];
253 
254  c.write(
255  s, mrpt::format("wp%03u_allowed_distance", i), wp.allowed_distance,
256  NP);
257  c.write(s, mrpt::format("wp%03u_allow_skip", i), wp.allow_skip, NP);
258  c.write(s, mrpt::format("wp%03u_target_x", i), wp.target.x, NP);
259  c.write(s, mrpt::format("wp%03u_target_y", i), wp.target.y, NP);
260  c.write(
261  s, mrpt::format("wp%03u_target_frame_id", i), wp.target_frame_id,
262  NP);
263  c.write(
264  s, mrpt::format("wp%03u_target_heading", i), wp.target_heading, NP);
265  }
266 }
267 
270 {
271  this->clear();
272 
273  const unsigned int N = c.read_int(s, "waypoint_count", 0, true);
274  waypoints.resize(N);
275 
276  for (unsigned int i = 0; i < N; i++)
277  {
278  auto& wp = waypoints[i];
279 
280  wp.allowed_distance = c.read_double(
281  s, mrpt::format("wp%03u_allowed_distance", i), 0, true);
282  wp.allow_skip =
283  c.read_bool(s, mrpt::format("wp%03u_allow_skip", i), true, true);
284  wp.target.x =
285  c.read_double(s, mrpt::format("wp%03u_target_x", i), 0, true);
286  wp.target.y =
287  c.read_double(s, mrpt::format("wp%03u_target_y", i), 0, true);
288  wp.target_frame_id = c.read_string(
289  s, mrpt::format("wp%03u_target_frame_id", i), "map", false);
290  double hd = c.read_double(
291  s, mrpt::format("wp%03u_target_heading", i),
293  wp.target_heading = (hd > 100) ? mrpt::nav::TWaypoint::INVALID_NUM : hd;
294  }
295 }
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:51
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:102
double RAD2DEG(const double x)
Radians to degrees.
double x
X,Y coordinates.
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:42
TWaypointSequence()
Ctor with default values.
Definition: TWaypoint.cpp:72
std::string getAsText() const
get in human-readable format
Definition: TWaypoint.cpp:48
used in getAsOpenglVisualization()
Definition: TWaypoint.h:69
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:192
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:88
A single waypoint within TWaypointSequence.
Definition: TWaypoint.h:22
STL namespace.
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
Definition: TWaypoint.h:112
void load(const mrpt::config::CConfigFileBase &c, const std::string &s)
Loads waypoints to a config file section.
Definition: TWaypoint.cpp:268
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:242
TWaypoint()
Ctor with default values.
Definition: TWaypoint.cpp:22
const GLubyte * c
Definition: glext.h:6313
GLint GLvoid * img
Definition: glext.h:3763
A waypoint with an execution status.
Definition: TWaypoint.h:108
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:140
GLsizei const GLchar ** string
Definition: glext.h:4101
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:156
TWaypointStatus & operator=(const TWaypoint &wp)
Definition: TWaypoint.cpp:97
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TWaypointStatusSequence()
Ctor with default values.
Definition: TWaypoint.cpp:111
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::math::TPoint2D target
[Must be set by the user] Coordinates of desired target location (world/global coordinates).
Definition: TWaypoint.h:27
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:74
double target_heading
[Default=any heading] Optionally, set to the desired orientation [radians] of the robot at this waypo...
Definition: TWaypoint.h:33
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:41
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:122
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
Definition: TWaypoint.h:136
static const int INVALID_NUM
The default value of fields (used to detect non-set values)
Definition: TWaypoint.h:65
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:145
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020