24 #include "../common/types.h" 25 #include "select_drive_mode.h" 28 #include "stop_drive_mode.h" 29 #include "escape_drive_mode.h" 30 #include "escape_potential_field_drive_mode.h" 31 #include "forward_drive_mode.h" 32 #include "backward_drive_mode.h" 33 #include "biward_drive_mode.h" 35 #include "forward_omni_drive_mode.h" 36 #include "escape_potential_field_omni_drive_mode.h" 39 #include "../search/og_laser.h" 41 #include <interfaces/MotorInterface.h> 42 #include <interfaces/NavigatorInterface.h> 43 #include <logging/logger.h> 44 #include <config/config.h> 45 #include <utils/math/angle.h> 71 if_colli_target_( target ),
73 cfg_escape_mode_( escape_mode ),
76 logger_->
log_debug(
"SelectDriveMode",
"(Constructor): Entering");
79 std::string drive_restriction = config->
get_string(
"/plugins/colli/drive_mode/restriction");
81 if ( drive_restriction ==
"omnidirectional" ) {
82 drive_restriction_ = colli_drive_restriction_t::omnidirectional;
83 }
else if ( drive_restriction ==
"differential" ) {
84 drive_restriction_ = colli_drive_restriction_t::differential;
86 throw fawkes::Exception(
"Drive restriction '%s' is unknown", drive_restriction.c_str());
89 logger_->
log_debug(
"SelectDriveMode",
"(Constructor): Creating Drive Mode Objects");
95 if ( drive_restriction_ == colli_drive_restriction_t::omnidirectional ) {
96 load_drive_modes_omnidirectional();
98 load_drive_modes_differential();
101 logger_->
log_debug(
"SelectDriveMode",
"(Constructor): Exiting");
107 logger_->
log_debug(
"SelectDriveMode",
"(Destructor): Entering");
108 for (
unsigned int i = 0; i < drive_modes_.size(); i++ )
109 delete drive_modes_[i];
110 logger_->
log_debug(
"SelectDriveMode",
"(Destructor): Exiting");
115 SelectDriveMode::load_drive_modes_differential()
118 if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
120 }
else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
123 logger_->
log_error(
"SelectDriveMode",
"Unknown escape drive mode. Using basic as default");
141 SelectDriveMode::load_drive_modes_omnidirectional()
144 if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
146 }
else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
150 logger_->
log_error(
"SelectDriveMode",
"Unknown escape drive mode. Using potential field omni as default");
205 return proposed_.
rot;
218 for (
unsigned int i = 0; i < drive_modes_.size(); i++ ) {
226 logger_->
log_error(
"SelectDriveMode",
"Can't find escape drive mode to set grid information");
237 for (
unsigned int i = 0; i < drive_modes_.size(); i++ ) {
245 logger_->
log_error(
"SelectDriveMode",
"Can't find escape drive mode to set laser information");
264 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
268 if ( escape ==
true ) {
269 if( escape_flag_ == 0
270 && if_motor_->
des_vx() != 0.f
271 && if_motor_->
des_vx() != 0.f
284 desired_mode = if_colli_target_->
drive_mode();
288 for (
unsigned int i = 0; i < drive_modes_.size(); i++ ) {
290 if ( drive_modes_[i]->get_drive_mode_name() == desired_mode
291 && drive_mode != 0 ) {
292 logger_->
log_error(
"SelectDriveMode",
"Error while selecting drive mode. There is more than one mode with the same name!!! Stopping!");
298 if ( drive_modes_[i]->get_drive_mode_name() == desired_mode
299 && drive_mode == 0 ) {
300 drive_mode = drive_modes_[i];
305 if ( drive_mode == 0 ) {
307 logger_->
log_error(
"SelectDriveMode",
"INVALID DRIVE MODE POINTER, stopping!");
308 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
319 if_motor_->
omega() );
322 if_colli_target_->
dest_y(),
339 && (fabs( proposed_.
x ) > fabs( if_colli_target_->
max_velocity() )) ) {
340 if ( proposed_.
x > 0.0 )
347 && (fabs( proposed_.
y ) > fabs( if_colli_target_->
max_velocity() )) ) {
348 if ( proposed_.
y > 0.0 )
355 && (fabs( proposed_.
rot ) > fabs( if_colli_target_->
max_rotation() )) ) {
356 if ( proposed_.
rot > 0.0 )
SelectDriveMode(MotorInterface *motor, NavigatorInterface *colli_target, Logger *logger, Configuration *config, colli_escape_mode_t escape_mode=colli_escape_mode_t::basic)
Constructor.
~SelectDriveMode()
Desctructor.
float max_velocity() const
Get max_velocity value.
float x
Translation in x-direction.
float odometry_position_x() const
Get odometry_position_x value.
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
void set_laser_data(std::vector< fawkes::polar_coord_2d_t > &laser_points)
search for the escape drive mode and hands over the given information to the escape drive mode This s...
float get_proposed_rot()
Returns the proposed rotation. After an update.
float get_proposed_trans_x()
Returns the proposed x translation.
Fawkes library namespace.
bool is_stop_at_target() const
Get stop_at_target value.
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_local_target(float x, float y)
Set local target point before update!
float dest_ori() const
Get dest_ori value.
float y
Translation in y-direction.
float vx() const
Get vx value.
float rot
Rotation around z-axis.
OrientationMode orientation_mode() const
Get orientation_mode value.
float odometry_orientation() const
Get odometry_orientation value.
float odometry_position_y() const
Get odometry_position_y value.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
float dest_y() const
Get dest_y value.
Class Escape-Drive-Module.
DriveMode drive_mode() const
Get drive_mode value.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
float vy() const
Get vy value.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
This is the SlowForward drive-module, for slow forward only movements.
Base class for exceptions in Fawkes.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
float omega() const
Get omega value.
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
search for the escape drive mode and hands over the given information to the escape drive mode This s...
float get_proposed_trans_y()
Returns the proposed y translation.
void update(bool escape=false)
Has to be called before the proposed values are called.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
This is the Forward drive-module, for forward only movements.
Moving not allowed constant.
Class Escape-Drive-Module.
This is the base class which calculates drive modes.
void set_local_trajec(float x, float y)
Set local trajectory point before update!
This is the SlowBackward drive-module, for slow backward only movements.
float des_vx() const
Get des_vx value.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float dest_x() const
Get dest_x value.
colli_escape_mode_t
Colli Escape modes.
Class Escape-Drive-Module.
float des_omega() const
Get des_omega value.
MotorInterface Fawkes BlackBoard Interface.
float get_proposed_rot()
Returns the proposed rotatio.
DriveMode
Drive modes enum.
Interface for configuration handling.
This is the SlowBiward drive-module.
float get_proposed_trans_x()
Returns the proposed translation. After an update.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
NavigatorInterface Fawkes BlackBoard Interface.
float max_rotation() const
Get max_rotation value.
float get_proposed_trans_y()
Returns the proposed translation. After an update.