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
34namespace 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 */
46RoboShape::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 */
140bool
142{
143 return is_round_;
144}
145
146/** Returns if the robot is angular.
147 * @return bool indicating if the robot is angular.
148 */
149bool
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 */
160bool
161RoboShape::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 */
171bool
172RoboShape::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 */
180float
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 */
189float
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 */
198float
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 */
207float
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 */
216float
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 */
225float
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 */
234float
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 */
243float
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 */
253float
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 */
314float
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 */
323float
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 */
337float
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 */
352float
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 */
366float
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 */
380float
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 */
394float
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 */
408float
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 */
417float
419{
420 return laser_offset_y_;
421}
422
423} // namespace fawkes
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
float get_robot_length_for_rad(float anglerad)
return the length of the robot for a specific angle
Definition: roboshape.cpp:254
float get_laser_offset_x()
Returns the laser offset in x direction of the robot.
Definition: roboshape.cpp:409
float get_radius()
Returns the radius of the robot if its round.
Definition: roboshape.cpp:324
bool is_angular_robot()
Returns if the robot is angular.
Definition: roboshape.cpp:150
bool is_robot_reading_for_deg(float angledeg, float length)
Check if the reading is 'in' the robot.
Definition: roboshape.cpp:172
float get_width_x()
Returns the width-x of the angular robot.
Definition: roboshape.cpp:353
float get_angle_left() const
Get angle to middle of the left side of the robot.
Definition: roboshape.cpp:217
float get_angle_back_left() const
Get angle to of the rear left corner robot.
Definition: roboshape.cpp:199
float get_angle_front_right() const
Get angle to the front right corner of the robot.
Definition: roboshape.cpp:190
float get_angle_right() const
Get angle to middle of the right side of the robot.
Definition: roboshape.cpp:226
float get_angle_back() const
Get angle to middle of the back side of the robot.
Definition: roboshape.cpp:244
RoboShape(const char *cfg_prefix, fawkes::Logger *logger, fawkes::Configuration *config)
Constructor.
Definition: roboshape.cpp:46
float get_robot_length_for_deg(float angledeg)
return the length of the robot for a specific angle
Definition: roboshape.cpp:315
float get_width_y()
Returns the width-y of the angular robot.
Definition: roboshape.cpp:367
~RoboShape()
Desctructor.
Definition: roboshape.cpp:133
float get_laser_offset_y()
Returns the laser offset in y direction of the robot.
Definition: roboshape.cpp:418
float get_angle_back_right() const
Get angle to of the rear right corner robot.
Definition: roboshape.cpp:208
float get_angle_front_left() const
Get angle to the front left corner of the robot.
Definition: roboshape.cpp:181
float get_angle_front() const
Get angle to middle of the front side of the robot.
Definition: roboshape.cpp:235
float get_complete_width_x()
Returns the complete x width of the angular robot.
Definition: roboshape.cpp:381
float get_complete_width_y()
Returns the complete x width of the angular robot.
Definition: roboshape.cpp:395
bool is_robot_reading_for_rad(float anglerad, float length)
Check if the reading is 'in' the robot.
Definition: roboshape.cpp:161
float get_complete_radius()
Returns the maximum radius of the robot if its round.
Definition: roboshape.cpp:338
bool is_round_robot()
Returns if the robot is round.
Definition: roboshape.cpp:141
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72