23 #include "roboshape.h" 25 #include <config/config.h> 26 #include <core/exception.h> 27 #include <logging/logger.h> 28 #include <utils/math/angle.h> 49 std::string cfg = cfg_prefix;
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();
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());
62 int shape = config->
get_int((cfg +
"shape").c_str());
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());
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_;
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_;
93 logger_->
log_info(
"RoboShape",
"Shape is angular!");
95 }
else if (shape == 2) {
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());
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_;
108 logger_->
log_info(
"RoboShape",
"Shape is round!");
113 "RoboShape: Loading RoboShape from ConfigFile failed! Invalid config value for roboshape");
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_);
183 return ang_front_left_;
192 return ang_front_right_;
201 return ang_back_left_;
210 return ang_back_right_;
269 float ray_x = cos(anglerad);
270 float ray_y = sin(anglerad);
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_
277 return (-b + sqrt(b * b - a * c)) / a;
284 if (anglerad >= ang_back_left_ || anglerad < ang_back_right_) {
286 return robot_to_back_ / cos(M_PI - fabs(anglerad));
288 }
else if (anglerad < ang_front_right_) {
290 return robot_to_right_ / cos(M_PI_2 + anglerad);
292 }
else if (anglerad < ang_front_left_) {
294 return robot_to_front_ / cos(anglerad);
296 }
else if (anglerad < ang_back_left_) {
298 return robot_to_left_ / cos(M_PI_2 - anglerad);
302 "RoboShape: Angles to corners of robot-shape do not cover the whole robot!");
306 throw fawkes::Exception(
"RoboShape: Cannot return the robolength for unspecific robot!");
329 logger_->
log_error(
"RoboShape",
"The Robot is not round!");
342 + std::max(std::max(width_add_front_, width_add_back_),
343 std::max(width_add_right_, width_add_left_)));
345 logger_->
log_error(
"RoboShape",
"Error: The Robot is not round!");
358 logger_->
log_error(
"RoboShape",
"The Robot is not angular!");
372 logger_->
log_error(
"RoboShape",
"The Robot is not angular!");
384 return (width_x_ + width_add_front_ + width_add_back_);
398 return (width_y_ + width_add_right_ + width_add_left_);
411 return laser_offset_x_;
420 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.