Fawkes API  Fawkes Development Version
roboshape.cpp
1 
2 /***************************************************************************
3  * roboshape.cpp - Class containing shape information of robot
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013-2014 Bahram Maleki-Fard
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "roboshape.h"
24 
25 #include <config/config.h>
26 #include <core/exception.h>
27 #include <logging/logger.h>
28 #include <utils/math/angle.h>
29 
30 #include <cmath>
31 #include <limits>
32 #include <string>
33 
34 namespace fawkes {
35 
36 /** @class RoboShape <plugins/colli/utils/rob/roboshape.h>
37  * This is a class containing all roboshape information.
38  * All methods have been implemented but round robots.
39  */
40 
41 /** Constructor.
42  * @param cfg_prefix The prefix of the config node, where the roboshape values are found
43  * @param logger Pointer to the fawkes logger
44  * @param config Pointer to the fawkes configuration.
45  */
46 RoboShape::RoboShape(const char *cfg_prefix, Logger *logger, Configuration *config)
47 {
48  logger_ = logger;
49  std::string cfg = cfg_prefix;
50 
51  is_round_ = is_angular_ = false;
52  radius_ = width_x_ = width_y_ = std::numeric_limits<float>::infinity();
53  laser_offset_x_ = laser_offset_y_ = std::numeric_limits<float>::infinity();
54  width_add_front_ = width_add_back_ = std::numeric_limits<float>::infinity();
55  width_add_right_ = width_add_left_ = std::numeric_limits<float>::infinity();
56 
57  width_add_front_ = config->get_float((cfg + "extension/front").c_str());
58  width_add_right_ = config->get_float((cfg + "extension/right").c_str());
59  width_add_back_ = config->get_float((cfg + "extension/back").c_str());
60  width_add_left_ = config->get_float((cfg + "extension/left").c_str());
61 
62  int shape = config->get_int((cfg + "shape").c_str());
63  if (shape == 1) {
64  // ANGULAR
65  is_angular_ = true;
66  is_round_ = false;
67  width_x_ = config->get_float((cfg + "angular/width_x").c_str());
68  width_y_ = config->get_float((cfg + "angular/width_y").c_str());
69  laser_offset_x_ = config->get_float((cfg + "angular/laser_offset_x_from_back").c_str());
70  laser_offset_y_ = config->get_float((cfg + "angular/laser_offset_y_from_left").c_str());
71 
72  float laser_to_back = laser_offset_x_;
73  float laser_to_left = laser_offset_y_;
74  float laser_to_right = width_y_ - laser_offset_y_;
75  float laser_to_front = width_x_ - laser_offset_x_;
76 
77  robot_to_back_ = laser_to_back + width_add_back_;
78  robot_to_left_ = laser_to_left + width_add_left_;
79  robot_to_right_ = laser_to_right + width_add_right_;
80  robot_to_front_ = laser_to_front + width_add_front_;
81 
82  // angles from laser to the edges of real robot dimension
83  // (might be more precise than the calculation below. TODO: check this)
84  //ang_front_left_ = normalize_mirror_rad( atan2( laser_to_left, laser_to_front ) );
85  //ang_front_right_ = normalize_mirror_rad( atan2( -laser_to_right, laser_to_front ) );
86  //ang_back_left_ = normalize_mirror_rad( atan2( laser_to_left, -laser_to_back ) );
87  //ang_back_right_ = normalize_mirror_rad( atan2( -laser_to_right, -laser_to_back ) );
88  //ang_left_ = normalize_mirror_rad( atan2( laser_to_left, laser_to_front - width_x_/2.f ) );
89  //ang_right_ = normalize_mirror_rad( atan2( -laser_to_right, laser_to_front - width_x_/2.f ) );
90  //ang_front_ = normalize_mirror_rad( atan2( laser_to_left - width_y_/2.f, laser_to_front ) );
91  //ang_back_ = normalize_mirror_rad( atan2( laser_to_left - width_y_/2.f, -laser_to_back ) );
92 
93  logger_->log_info("RoboShape", "Shape is angular!");
94 
95  } else if (shape == 2) {
96  // ROUND
97  is_angular_ = false;
98  is_round_ = true;
99  radius_ = config->get_float((cfg + "round/radius").c_str());
100  laser_offset_x_ = config->get_float((cfg + "round/laser_offset_x_from_middle").c_str());
101  laser_offset_y_ = config->get_float((cfg + "round/laser_offset_y_from_middle").c_str());
102 
103  robot_to_back_ = radius_ + laser_offset_x_ + width_add_back_;
104  robot_to_front_ = radius_ - laser_offset_x_ + width_add_front_;
105  robot_to_left_ = radius_ - laser_offset_y_ + width_add_left_;
106  robot_to_right_ = radius_ + laser_offset_y_ + width_add_right_;
107 
108  logger_->log_info("RoboShape", "Shape is round!");
109 
110  } else {
111  // WRONG FORMAT!!!
112  throw fawkes::Exception(
113  "RoboShape: Loading RoboShape from ConfigFile failed! Invalid config value for roboshape");
114  }
115 
116  logger_->log_info("RoboShape", "|#--> (m) is to front: %f", robot_to_front_);
117  logger_->log_info("RoboShape", "|#--> (m) is to right: %f", robot_to_right_);
118  logger_->log_info("RoboShape", "|#--> (m) is to left : %f", robot_to_left_);
119  logger_->log_info("RoboShape", "+#--> (m) is to back : %f", robot_to_back_);
120 
121  // angles from laser to edges of the robot extension
122  ang_front_left_ = normalize_mirror_rad(atan2(robot_to_left_, robot_to_front_));
123  ang_front_right_ = normalize_mirror_rad(atan2(-robot_to_right_, robot_to_front_));
124  ang_back_left_ = normalize_mirror_rad(atan2(robot_to_left_, -robot_to_back_));
125  ang_back_right_ = normalize_mirror_rad(atan2(-robot_to_right_, -robot_to_back_));
126  ang_left_ = normalize_mirror_rad(atan2(robot_to_left_, robot_to_front_ - width_x_ / 2.f));
127  ang_right_ = normalize_mirror_rad(atan2(-robot_to_right_, robot_to_front_ - width_x_ / 2.f));
128  ang_front_ = normalize_mirror_rad(atan2(robot_to_left_ - width_y_ / 2.f, robot_to_front_));
129  ang_back_ = normalize_mirror_rad(atan2(robot_to_left_ - width_y_ / 2.f, -robot_to_back_));
130 }
131 
132 /** Desctructor. */
134 {
135 }
136 
137 /** Returns if the robot is round.
138  * @return bool indicating if the robot is round.
139  */
140 bool
142 {
143  return is_round_;
144 }
145 
146 /** Returns if the robot is angular.
147  * @return bool indicating if the robot is angular.
148  */
149 bool
151 {
152  return is_angular_;
153 }
154 
155 /** Returns, if a reading length is _in_ the robot.
156  * @param anglerad is float containing the angle of the reading in radians.
157  * @param length containing the length of the reading.
158  * @return if the reading is in the robot.
159  */
160 bool
161 RoboShape::is_robot_reading_for_rad(float anglerad, float length)
162 {
163  return (length < get_robot_length_for_rad(anglerad));
164 }
165 
166 /** Returns, if a reading length is _in_ the robot.
167  * @param angledeg is float containing the angle of the reading in degree.
168  * @param length containing the length of the reading.
169  * @return if the reading is in the robot.
170  */
171 bool
172 RoboShape::is_robot_reading_for_deg(float angledeg, float length)
173 {
174  return is_robot_reading_for_rad(deg2rad(angledeg), length);
175 }
176 
177 /** Get angle to the front left corner of the robot
178  * @return angle in radians
179  */
180 float
182 {
183  return ang_front_left_;
184 }
185 
186 /** Get angle to the front right corner of the robot
187  * @return angle in radians
188  */
189 float
191 {
192  return ang_front_right_;
193 }
194 
195 /** Get angle to the rear left corner of the robot
196  * @return angle in radians
197  */
198 float
200 {
201  return ang_back_left_;
202 }
203 
204 /** Get angle to the rear right corner of the robot
205  * @return angle in radians
206  */
207 float
209 {
210  return ang_back_right_;
211 }
212 
213 /** Get angle to middle of the left side of the robot
214  * @return angle in radians
215  */
216 float
218 {
219  return ang_left_;
220 }
221 
222 /** Get angle to middle of the right side of the robot
223  * @return angle in radians
224  */
225 float
227 {
228  return ang_right_;
229 }
230 
231 /** Get angle to middle of the front side of the robot
232  * @return angle in radians
233  */
234 float
236 {
237  return ang_front_;
238 }
239 
240 /** Get angle to middle of the rear side of the robot
241  * @return angle in radians
242  */
243 float
245 {
246  return ang_back_;
247 }
248 
249 /** Returns the robots length for a specific angle.
250  * @param anglerad is the angle in radians.
251  * @return the length in this direction.
252  */
253 float
255 {
256  anglerad = normalize_mirror_rad(anglerad);
257 
258  if (is_round_robot()) {
259  /* use quadratic equation to get intersection point of ray to circle.
260  * The ray origins at the laser with angle "anglerad" and is a unit_vector.
261  * Consider robot-center as (0,0), we have an equation of:
262  * length(v_laser + k*ray) = radius + expansion
263  * with v_laser = vector(laser_offset_x_, laser_offset_y_).
264  * "k" is the length from the laser to the robot edge at angle "anglerad".
265  *
266  * Transform that equation, i.e. resolve "length(..)" and you get a
267  * quadratic equation of the kind "ax^2 + 2bx + c = 0".
268  */
269  float ray_x = cos(anglerad); // unit vector!
270  float ray_y = sin(anglerad);
271 
272  float a = ray_x * ray_x + ray_y * ray_y;
273  float b = ray_x * laser_offset_x_ + ray_y * laser_offset_y_;
274  static float c = laser_offset_x_ * laser_offset_x_ + laser_offset_y_ * laser_offset_y_
276 
277  return (-b + sqrt(b * b - a * c)) / a;
278 
279  } else if (is_angular_robot()) {
280  /* check all the quadrants in which the target angles lies. The quadrants are spanned
281  * by the angles from the center of the robot to its 4 corners. Use "cos(a) = adjacent / hypothenuse",
282  * we are looking for the length of the hypothenuse here.
283  */
284  if (anglerad >= ang_back_left_ || anglerad < ang_back_right_) {
285  // bottom quadrant; fabs(anglerad) > M_PI_2
286  return robot_to_back_ / cos(M_PI - fabs(anglerad));
287 
288  } else if (anglerad < ang_front_right_) {
289  // right quadrant; -M_PI < anglerad < 0
290  return robot_to_right_ / cos(M_PI_2 + anglerad);
291 
292  } else if (anglerad < ang_front_left_) {
293  // top quadrant; -M_PI_2 < anglerad < M_PI_2
294  return robot_to_front_ / cos(anglerad);
295 
296  } else if (anglerad < ang_back_left_) {
297  // left quadrant; 0 < anglerad < M_PI
298  return robot_to_left_ / cos(M_PI_2 - anglerad);
299 
300  } else {
301  throw fawkes::Exception(
302  "RoboShape: Angles to corners of robot-shape do not cover the whole robot!");
303  }
304 
305  } else {
306  throw fawkes::Exception("RoboShape: Cannot return the robolength for unspecific robot!");
307  }
308 }
309 
310 /** Returns the robots length for a specific angle.
311  * @param angledeg is the angle in degree.
312  * @return the length in this direction.
313  */
314 float
316 {
317  return get_robot_length_for_rad(deg2rad(angledeg));
318 }
319 
320 /** Returns the radius of the robot if its round.
321  * @return radius of the round robot
322  */
323 float
325 {
326  if (is_round_robot())
327  return radius_;
328  else
329  logger_->log_error("RoboShape", "The Robot is not round!");
330 
331  return 0.f;
332 }
333 
334 /** Returns the maximum radius of the robot if its round.
335  * @return maximum radius of the round robot
336  */
337 float
339 {
340  if (is_round_robot())
341  return (radius_
342  + std::max(std::max(width_add_front_, width_add_back_),
343  std::max(width_add_right_, width_add_left_)));
344  else
345  logger_->log_error("RoboShape", "Error: The Robot is not round!");
346 
347  return 0.f;
348 }
349 /** Returns the width-x of the angular robot.
350  * @return only the robot x width.
351  */
352 float
354 {
355  if (is_angular_robot())
356  return width_x_;
357  else
358  logger_->log_error("RoboShape", "The Robot is not angular!");
359 
360  return 0.f;
361 }
362 
363 /** Returns the width-y of the angular robot.
364  * @return only the robot y width.
365  */
366 float
368 {
369  if (is_angular_robot())
370  return width_y_;
371  else
372  logger_->log_error("RoboShape", "The Robot is not angular!");
373 
374  return 0.f;
375 }
376 
377 /** Returns the complete x width of the angular robot.
378  * @return the complete x width.
379  */
380 float
382 {
383  if (is_angular_robot())
384  return (width_x_ + width_add_front_ + width_add_back_);
385  else
386  return 2.f * get_complete_radius();
387 
388  return 0.f;
389 }
390 
391 /** Returns the complete y width of the angular robot.
392  * @return the complete y width.
393  */
394 float
396 {
397  if (is_angular_robot())
398  return (width_y_ + width_add_right_ + width_add_left_);
399  else
400  return 2.f * get_complete_radius();
401 
402  return 0.f;
403 }
404 
405 /** Returns the laser offset in x direction of the robot.
406  * @return the laser offset in x direction.
407  */
408 float
410 {
411  return laser_offset_x_;
412 }
413 
414 /** Returns the laser offset in y direction of the robot.
415  * @return the laser offset in y direction.
416  */
417 float
419 {
420  return laser_offset_y_;
421 }
422 
423 } // namespace fawkes
fawkes::RoboShape::get_angle_left
float get_angle_left() const
Get angle to middle of the left side of the robot.
Definition: roboshape.cpp:221
fawkes::RoboShape::get_complete_width_x
float get_complete_width_x()
Returns the complete x width of the angular robot.
Definition: roboshape.cpp:385
fawkes::RoboShape::get_width_y
float get_width_y()
Returns the width-y of the angular robot.
Definition: roboshape.cpp:371
fawkes::RoboShape::get_laser_offset_x
float get_laser_offset_x()
Returns the laser offset in x direction of the robot.
Definition: roboshape.cpp:413
fawkes::RoboShape::is_robot_reading_for_rad
bool is_robot_reading_for_rad(float anglerad, float length)
Check if the reading is 'in' the robot.
Definition: roboshape.cpp:165
fawkes::RoboShape::get_angle_front_left
float get_angle_front_left() const
Get angle to the front left corner of the robot.
Definition: roboshape.cpp:185
fawkes::RoboShape::RoboShape
RoboShape(const char *cfg_prefix, fawkes::Logger *logger, fawkes::Configuration *config)
Constructor.
Definition: roboshape.cpp:50
fawkes::RoboShape::is_round_robot
bool is_round_robot()
Returns if the robot is round.
Definition: roboshape.cpp:145
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::RoboShape::is_angular_robot
bool is_angular_robot()
Returns if the robot is angular.
Definition: roboshape.cpp:154
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
fawkes::RoboShape::is_robot_reading_for_deg
bool is_robot_reading_for_deg(float angledeg, float length)
Check if the reading is 'in' the robot.
Definition: roboshape.cpp:176
fawkes::RoboShape::get_width_x
float get_width_x()
Returns the width-x of the angular robot.
Definition: roboshape.cpp:357
fawkes::RoboShape::get_angle_back_right
float get_angle_back_right() const
Get angle to of the rear right corner robot.
Definition: roboshape.cpp:212
fawkes::RoboShape::get_angle_back
float get_angle_back() const
Get angle to middle of the back side of the robot.
Definition: roboshape.cpp:248
fawkes::RoboShape::get_angle_right
float get_angle_right() const
Get angle to middle of the right side of the robot.
Definition: roboshape.cpp:230
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes::RoboShape::get_complete_radius
float get_complete_radius()
Returns the maximum radius of the robot if its round.
Definition: roboshape.cpp:342
fawkes::RoboShape::get_complete_width_y
float get_complete_width_y()
Returns the complete x width of the angular robot.
Definition: roboshape.cpp:399
fawkes
fawkes::RoboShape::get_radius
float get_radius()
Returns the radius of the robot if its round.
Definition: roboshape.cpp:328
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:40
fawkes::RoboShape::get_angle_back_left
float get_angle_back_left() const
Get angle to of the rear left corner robot.
Definition: roboshape.cpp:203
fawkes::RoboShape::get_robot_length_for_rad
float get_robot_length_for_rad(float anglerad)
return the length of the robot for a specific angle
Definition: roboshape.cpp:258
fawkes::RoboShape::~RoboShape
~RoboShape()
Desctructor.
Definition: roboshape.cpp:137
fawkes::normalize_mirror_rad
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:76
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::RoboShape::get_angle_front
float get_angle_front() const
Get angle to middle of the front side of the robot.
Definition: roboshape.cpp:239
fawkes::RoboShape::get_angle_front_right
float get_angle_front_right() const
Get angle to the front right corner of the robot.
Definition: roboshape.cpp:194
fawkes::RoboShape::get_robot_length_for_deg
float get_robot_length_for_deg(float angledeg)
return the length of the robot for a specific angle
Definition: roboshape.cpp:319
fawkes::RoboShape::get_laser_offset_y
float get_laser_offset_y()
Returns the laser offset in y direction of the robot.
Definition: roboshape.cpp:422
fawkes::Exception
Definition: exception.h:39