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