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