Main MRPT website > C++ reference for MRPT 1.5.5
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-2017, 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 const double TWaypoint::INVALID_NUM = std::numeric_limits<double>::max();
22 
23 // TWaypoint ==========
25  target(INVALID_NUM,INVALID_NUM),
26  target_heading(INVALID_NUM),
27  target_frame_id("map"),
28  allowed_distance(INVALID_NUM),
29  allow_skip(true)
30 {
31 }
32 
33 TWaypoint::TWaypoint(double target_x, double target_y, double allowed_distance_, 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
45  (target.x!=INVALID_NUM) &&
46  (target.y!=INVALID_NUM) &&
48 }
49 
51 {
52  std::string s;
54  s+=mrpt::format("target=(%8.03f,%8.03f) ",target.x,target.y);
55  else s+="target=(**Coordinates not set!!**) ";
56 
59  else s+=" (heading: any) ";
60 
62  s+=mrpt::format("allowed_dist=%8.03f ",allowed_distance);
63  else s+=" (**allowed_distance not set!!**) ";
64 
65  s+= (allow_skip ? " allow_skip: YES" : " allow_skip: NO ");
66 
67  return s;
68 }
69 
70 
71 // TWaypointSequence ==========
73 {
74 }
75 
76 // Gets navigation params as a human-readable format:
78 {
79  string s;
80  s+=mrpt::format("List of %u waypoints:\n", static_cast<unsigned int>(waypoints.size()) );
81  unsigned int i=0;
82  for (const auto &wp : waypoints) {
83  s+= mrpt::format(" #%3u: ",i++);
84  s+= wp.getAsText();
85  s+= "\n";
86  }
87  return s;
88 }
89 
90 // TWaypointStatus ==========
92  reached(false),
93  skipped(false),
94  timestamp_reach(INVALID_TIMESTAMP),
95  counter_seen_reachable(0)
96 {
97 }
99 {
100  TWaypoint::operator =(wp);
101  return *this;
102 }
104 {
106  s += mrpt::format(" reached=%s", (reached ? "YES":"NO ") );;
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(TWaypoint::INVALID_NUM,TWaypoint::INVALID_NUM,TWaypoint::INVALID_NUM)
117 {
118 }
119 
121 {
122  string s;
123  s+=mrpt::format("Status for %u waypoints:\n", static_cast<unsigned int>(waypoints.size()) );
124  unsigned int i=0;
125  for (const auto &wp : waypoints) {
126  s+= mrpt::format(" #%3u: ",i++);
127  s+= wp.getAsText();
128  s+= "\n";
129  }
130  s+=mrpt::format(" final_goal_reached:%s waypoint_index_current_goal=%d\n", (final_goal_reached ? "YES":"NO "), waypoint_index_current_goal );
131  return s;
132 }
133 
135  outter_radius(.3), inner_radius(.2),
136  outter_radius_non_skippable(.3), inner_radius_non_skippable(.0),
137  outter_radius_reached(.2), inner_radius_reached(.1),
138  heading_arrow_len(1.0),
139  color_regular(mrpt::utils::TColor(0x00, 0x00, 0xff)),
140  color_current_goal(mrpt::utils::TColor(0xff, 0x00, 0x20)),
141  color_reached(mrpt::utils::TColor(0x00, 0x00, 0xc0,0xd0)),
142  show_labels(true)
143 {
144 }
145 
146 
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 : params.outter_radius_non_skippable,
155  p.allow_skip ? params.inner_radius : params.inner_radius_non_skippable,
156  15);
157  gl_pt->setLocation(p.target.x, p.target.y, 0.01);
158  gl_pt->setColor_u8(params.color_regular);
159  if (params.show_labels)
160  {
161  gl_pt->setName(mrpt::format("WayPt #%2u", idx));
162  gl_pt->enableShowName(true);
163  }
164  obj.insert(gl_pt);
165 
166  if (p.target_heading != TWaypoint::INVALID_NUM)
167  {
168  auto o = mrpt::opengl::CArrow::Create(0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
169  o->setPose(mrpt::poses::CPose3D(p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
170  obj.insert(o);
171  }
172  ++idx;
173  }
174 }
175 
177 {
178  obj.clear();
179  {
180  unsigned int idx = 0;
181  for (const auto &p : waypoints)
182  {
183  const bool is_cur_goal = (int(idx) == waypoint_index_current_goal);
184 
185  mrpt::opengl::CDiskPtr gl_pt = mrpt::opengl::CDisk::Create(
186  p.reached ? params.outter_radius_reached : (p.allow_skip ? params.outter_radius : params.outter_radius_non_skippable),
187  p.reached ? params.inner_radius_reached : (p.allow_skip ? params.inner_radius : params.inner_radius_non_skippable),
188  15
189  );
190  gl_pt->setLocation(p.target.x, p.target.y, 0.01);
191  if (params.show_labels)
192  {
193  gl_pt->setName(mrpt::format("WayPt #%2u Reach:%s", idx, p.reached ? "YES" : "NO"));
194  gl_pt->enableShowName(true);
195  }
196  gl_pt->setColor_u8(is_cur_goal ? params.color_current_goal : (p.reached ? params.color_reached : params.color_regular));
197  obj.insert(gl_pt);
198 
199  if (p.target_heading != TWaypoint::INVALID_NUM)
200  {
201  auto o = mrpt::opengl::CArrow::Create(0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
202  o->setPose(mrpt::poses::CPose3D(p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
203  obj.insert(o);
204  }
205  ++idx;
206  }
207  }
208 }
209 
211 {
212  const unsigned int N = waypoints.size();
213  c.write(s, "waypoint_count", N);
214 
215  const int NP = 27; // name padding
216 
217  for (unsigned int i = 0; i < N; i++)
218  {
219  const auto &wp = waypoints[i];
220 
221  c.write(s, mrpt::format("wp%03u_allowed_distance", i), wp.allowed_distance, NP);
222  c.write(s, mrpt::format("wp%03u_allow_skip", i), wp.allow_skip, NP);
223  c.write(s, mrpt::format("wp%03u_target_x", i), wp.target.x, NP);
224  c.write(s, mrpt::format("wp%03u_target_y", i), wp.target.y, NP);
225  c.write(s, mrpt::format("wp%03u_target_frame_id", i), wp.target_frame_id, NP);
226  c.write(s, mrpt::format("wp%03u_target_heading", i), wp.target_heading, NP);
227  }
228 }
229 
231 {
232  this->clear();
233 
234  const unsigned int N = c.read_int(s, "waypoint_count", 0, true);
235  waypoints.resize(N);
236 
237  for (unsigned int i = 0; i < N; i++)
238  {
239  auto &wp = waypoints[i];
240 
241  wp.allowed_distance = c.read_double(s, mrpt::format("wp%03u_allowed_distance", i), 0, true);
242  wp.allow_skip = c.read_bool(s, mrpt::format("wp%03u_allow_skip", i), true, true);
243  wp.target.x = c.read_double(s, mrpt::format("wp%03u_target_x", i), 0, true);
244  wp.target.y = c.read_double(s, mrpt::format("wp%03u_target_y", i), 0, true);
245  wp.target_frame_id = c.read_string(s, mrpt::format("wp%03u_target_frame_id", i), "map", false);
246  double hd = c.read_double(s, mrpt::format("wp%03u_target_heading", i), mrpt::nav::TWaypoint::INVALID_NUM);
247  wp.target_heading = (hd > 100) ? mrpt::nav::TWaypoint::INVALID_NUM : hd;
248  }
249 }
250 
void load(const mrpt::utils::CConfigFileBase &c, const std::string &s)
Loads waypoints to a config file section.
Definition: TWaypoint.cpp:230
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:44
double y
X,Y coordinates.
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:103
static CArrowPtr Create()
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:32
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
static const double INVALID_NUM
The default value of fields (used to detect non-set values)
Definition: TWaypoint.h:51
void save(mrpt::utils::CConfigFileBase &c, const std::string &s) const
Saves waypoints to a config file section.
Definition: TWaypoint.cpp:210
std::string getAsText() const
get in human-readable format
Definition: TWaypoint.cpp:50
used in getAsOpenglVisualization()
Definition: TWaypoint.h:55
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:176
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:73
A single waypoint within TWaypointSequence.
Definition: TWaypoint.h:25
STL namespace.
GLdouble s
Definition: glext.h:3602
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
This class allows loading and storing values and vectors of different types from a configuration text...
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
Definition: TWaypoint.h:90
static CDiskPtr Create()
double RAD2DEG(const double x)
Radians to degrees.
TWaypoint()
Ctor with default values.
Definition: TWaypoint.cpp:24
const GLubyte * c
Definition: glext.h:5590
A waypoint with an execution status.
Definition: TWaypoint.h:88
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:107
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
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:147
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
TWaypointStatus & operator=(const TWaypoint &wp)
Definition: TWaypoint.cpp:98
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:72
mrpt::math::TPoint2D target
[Must be set by the user] Coordinates of desired target location (world/global coordinates).
Definition: TWaypoint.h:29
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: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:37
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:120
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
Definition: TWaypoint.h:105
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:111
GLfloat GLfloat p
Definition: glext.h:5587
GLenum const GLfloat * params
Definition: glext.h:3514



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019