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> 54 std::string cfg = cfg_prefix;
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;
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_) ) ) {
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());
82 int shape = config->
get_int((cfg +
"shape").c_str());
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());
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_;
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_;
113 logger_->
log_info(
"RoboShape",
"Shape is angular!");
115 }
else if ( shape == 2 ) {
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());
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_;
128 logger_->
log_info(
"RoboShape",
"Shape is round!");
132 throw fawkes::Exception(
"RoboShape: Loading RoboShape from ConfigFile failed! Invalid config value for roboshape");
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_);
203 return ang_front_left_;
212 return ang_front_right_;
221 return ang_back_left_;
230 return ang_back_right_;
289 float ray_x = cos(anglerad);
290 float ray_y = sin(anglerad);
292 float a = ray_x*ray_x + ray_y*ray_y;
293 float b = ray_x*laser_offset_x_ + ray_y*laser_offset_y_;
296 return ( -b + sqrt(b*b - a*c) ) / a;
303 if( anglerad >= ang_back_left_ || anglerad < ang_back_right_ ) {
305 return robot_to_back_ / cos( M_PI - fabs(anglerad) );
307 }
else if( anglerad < ang_front_right_ ) {
309 return robot_to_right_ / cos( M_PI_2 + anglerad );
311 }
else if( anglerad < ang_front_left_ ) {
313 return robot_to_front_ / cos( anglerad );
315 }
else if( anglerad < ang_back_left_ ) {
317 return robot_to_left_ / cos( M_PI_2 - anglerad);
320 throw fawkes::Exception(
"RoboShape: Angles to corners of robot-shape do not cover the whole robot!");
324 throw fawkes::Exception(
"RoboShape: Cannot return the robolength for unspecific robot!");
347 logger_->
log_error(
"RoboShape",
"The Robot is not round!");
359 return ( radius_ + std::max( std::max(width_add_front_, width_add_back_),
360 std::max(width_add_right_, width_add_left_) ) );
362 logger_->
log_error(
"RoboShape",
"Error: The Robot is not round!");
375 logger_->
log_error(
"RoboShape",
"The Robot is not angular!");
389 logger_->
log_error(
"RoboShape",
"The Robot is not angular!");
401 return ( width_x_ + width_add_front_ + width_add_back_ );
416 return ( width_y_ + width_add_right_ + width_add_left_ );
429 return laser_offset_x_;
438 return laser_offset_y_;
float get_radius()
Returns the radius of the robot if its round.
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
float get_robot_length_for_rad(float anglerad)
return the length of the robot for a specific angle
float get_width_x()
Returns the width-x of the angular robot.
Fawkes library namespace.
float get_complete_width_y()
Returns the complete x width of the angular robot.
float get_angle_right() const
Get angle to middle of the right side of the robot.
bool is_robot_reading_for_rad(float anglerad, float length)
Check if the reading is 'in' the robot.
float get_laser_offset_y()
Returns the laser offset in y direction of the robot.
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.
float get_angle_left() const
Get angle to middle of the left side of the robot.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Base class for exceptions in Fawkes.
float get_complete_width_x()
Returns the complete x width of the angular robot.
RoboShape(const char *cfg_prefix, fawkes::Logger *logger, fawkes::Configuration *config)
Constructor.
float get_angle_front_right() const
Get angle to the front right corner of the robot.
float get_width_y()
Returns the width-y of the angular robot.
float get_laser_offset_x()
Returns the laser offset in x direction of the robot.
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.
float get_angle_front_left() const
Get angle to the front left corner of the robot.
float get_complete_radius()
Returns the maximum radius of the robot if its round.
bool is_round_robot()
Returns if the robot is round.
bool is_angular_robot()
Returns if the robot is angular.
bool is_robot_reading_for_deg(float angledeg, float length)
Check if the reading is 'in' the robot.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Interface for configuration handling.
float get_angle_back_left() const
Get angle to of the rear left corner robot.
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.