23 #include "roboshape.h" 25 #include <logging/logger.h> 26 #include <config/config.h> 27 #include <core/exception.h> 28 #include <utils/math/angle.h> 50 RoboShape::RoboShape(
const char * cfg_prefix,
55 std::string cfg = cfg_prefix;
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();
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());
68 int shape = config->
get_int((cfg +
"shape").c_str());
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());
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_;
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_;
99 logger_->log_info(
"RoboShape",
"Shape is angular!");
101 }
else if ( shape == 2 ) {
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());
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_;
114 logger_->log_info(
"RoboShape",
"Shape is round!");
118 throw fawkes::Exception(
"RoboShape: Loading RoboShape from ConfigFile failed! Invalid config value for roboshape");
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_);
139 RoboShape::~RoboShape()
147 RoboShape::is_round_robot()
156 RoboShape::is_angular_robot()
167 RoboShape::is_robot_reading_for_rad(
float anglerad,
float length )
169 return (length < get_robot_length_for_rad( anglerad ));
178 RoboShape::is_robot_reading_for_deg(
float angledeg,
float length )
180 return is_robot_reading_for_rad(
deg2rad( angledeg ), length );
187 RoboShape::get_angle_front_left()
const 189 return ang_front_left_;
196 RoboShape::get_angle_front_right()
const 198 return ang_front_right_;
205 RoboShape::get_angle_back_left()
const 207 return ang_back_left_;
214 RoboShape::get_angle_back_right()
const 216 return ang_back_right_;
223 RoboShape::get_angle_left()
const 232 RoboShape::get_angle_right()
const 241 RoboShape::get_angle_front()
const 250 RoboShape::get_angle_back()
const 260 RoboShape::get_robot_length_for_rad(
float anglerad )
264 if( is_round_robot() ) {
275 float ray_x = cos(anglerad);
276 float ray_y = sin(anglerad);
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();
282 return ( -b + sqrt(b*b - a*c) ) / a;
284 }
else if( is_angular_robot() ) {
289 if( anglerad >= ang_back_left_ || anglerad < ang_back_right_ ) {
291 return robot_to_back_ / cos( M_PI - fabs(anglerad) );
293 }
else if( anglerad < ang_front_right_ ) {
295 return robot_to_right_ / cos( M_PI_2 + anglerad );
297 }
else if( anglerad < ang_front_left_ ) {
299 return robot_to_front_ / cos( anglerad );
301 }
else if( anglerad < ang_back_left_ ) {
303 return robot_to_left_ / cos( M_PI_2 - anglerad);
306 throw fawkes::Exception(
"RoboShape: Angles to corners of robot-shape do not cover the whole robot!");
310 throw fawkes::Exception(
"RoboShape: Cannot return the robolength for unspecific robot!");
319 RoboShape::get_robot_length_for_deg(
float angledeg )
321 return get_robot_length_for_rad(
deg2rad( angledeg ) );
328 RoboShape::get_radius()
330 if ( is_round_robot() )
333 logger_->log_error(
"RoboShape",
"The Robot is not round!");
342 RoboShape::get_complete_radius()
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_) ) );
348 logger_->log_error(
"RoboShape",
"Error: The Robot is not round!");
356 RoboShape::get_width_x()
358 if ( is_angular_robot() )
361 logger_->log_error(
"RoboShape",
"The Robot is not angular!");
370 RoboShape::get_width_y()
372 if ( is_angular_robot() )
375 logger_->log_error(
"RoboShape",
"The Robot is not angular!");
384 RoboShape::get_complete_width_x()
386 if ( is_angular_robot() )
387 return ( width_x_ + width_add_front_ + width_add_back_ );
389 return 2.f*get_complete_radius();
399 RoboShape::get_complete_width_y()
401 if ( is_angular_robot() )
402 return ( width_y_ + width_add_right_ + width_add_left_ );
404 return 2.f*get_complete_radius();
413 RoboShape::get_laser_offset_x()
415 return laser_offset_x_;
422 RoboShape::get_laser_offset_y()
424 return laser_offset_y_;
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).
Base class for exceptions in Fawkes.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.