24 #include "select_drive_mode.h"
27 #include "backward_drive_mode.h"
28 #include "biward_drive_mode.h"
29 #include "escape_drive_mode.h"
30 #include "escape_potential_field_drive_mode.h"
31 #include "escape_potential_field_omni_drive_mode.h"
32 #include "forward_drive_mode.h"
33 #include "forward_omni_drive_mode.h"
34 #include "stop_drive_mode.h"
37 #include "../search/og_laser.h"
39 #include <config/config.h>
40 #include <interfaces/MotorInterface.h>
41 #include <interfaces/NavigatorInterface.h>
42 #include <logging/logger.h>
43 #include <utils/math/angle.h>
65 if_colli_target_(target),
67 cfg_escape_mode_(escape_mode),
70 logger_->
log_debug(
"SelectDriveMode",
"(Constructor): Entering");
73 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
75 std::string drive_restriction = config->
get_string(
"/plugins/colli/drive_mode/restriction");
77 if (drive_restriction ==
"omnidirectional") {
78 drive_restriction_ = colli_drive_restriction_t::omnidirectional;
79 }
else if (drive_restriction ==
"differential") {
80 drive_restriction_ = colli_drive_restriction_t::differential;
82 throw fawkes::Exception(
"Drive restriction '%s' is unknown", drive_restriction.c_str());
85 logger_->
log_debug(
"SelectDriveMode",
"(Constructor): Creating Drive Mode Objects");
91 if (drive_restriction_ == colli_drive_restriction_t::omnidirectional) {
92 load_drive_modes_omnidirectional();
94 load_drive_modes_differential();
97 logger_->
log_debug(
"SelectDriveMode",
"(Constructor): Exiting");
103 logger_->
log_debug(
"SelectDriveMode",
"(Destructor): Entering");
104 for (
unsigned int i = 0; i < drive_modes_.size(); i++)
105 delete drive_modes_[i];
106 logger_->
log_debug(
"SelectDriveMode",
"(Destructor): Exiting");
110 SelectDriveMode::load_drive_modes_differential()
113 if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
115 }
else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
116 drive_modes_.push_back(
new EscapeDriveModule(logger_, config_));
118 logger_->
log_error(
"SelectDriveMode",
"Unknown escape drive mode. Using basic as default");
119 drive_modes_.push_back(
new EscapeDriveModule(logger_, config_));
123 ForwardDriveModule *forward =
new ForwardDriveModule(logger_, config_);
124 drive_modes_.push_back(forward);
127 BackwardDriveModule *backward =
new BackwardDriveModule(logger_, config_);
128 drive_modes_.push_back(backward);
131 drive_modes_.push_back(
new BiwardDriveModule(forward, backward, logger_, config_));
135 SelectDriveMode::load_drive_modes_omnidirectional()
138 if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
139 drive_modes_.push_back(
new EscapePotentialFieldOmniDriveModule(logger_, config_));
140 }
else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
142 drive_modes_.push_back(
new EscapeDriveModule(logger_, config_));
145 "Unknown escape drive mode. "
146 "Using potential field omni as default");
147 drive_modes_.push_back(
new EscapePotentialFieldOmniDriveModule(logger_, config_));
150 ForwardOmniDriveModule *forward =
new ForwardOmniDriveModule(logger_, config_);
151 drive_modes_.push_back(forward);
200 return proposed_.
rot;
213 for (
unsigned int i = 0; i < drive_modes_.size(); i++) {
222 logger_->
log_error(
"SelectDriveMode",
"Can't find escape drive mode to set grid information");
233 for (
unsigned int i = 0; i < drive_modes_.size(); i++) {
241 logger_->
log_error(
"SelectDriveMode",
"Can't find escape drive mode to set laser information");
260 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
264 if (escape ==
true) {
265 if (escape_flag_ == 0 && if_motor_->
des_vx() != 0.f && if_motor_->
des_vy() != 0.f
278 desired_mode = if_colli_target_->
drive_mode();
282 for (
unsigned int i = 0; i < drive_modes_.size(); i++) {
284 if (drive_modes_[i]->get_drive_mode_name() == desired_mode && drive_mode != 0) {
286 "Error while selecting drive mode. There is more than one mode with the "
287 "same name!!! Stopping!");
293 if (drive_modes_[i]->get_drive_mode_name() == desired_mode && drive_mode == 0) {
294 drive_mode = drive_modes_[i];
298 if (drive_mode == 0) {
300 logger_->
log_error(
"SelectDriveMode",
"INVALID DRIVE MODE POINTER, stopping!");
301 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
313 if_colli_target_->
dest_y(),
331 && (fabs(proposed_.
x) > fabs(if_colli_target_->
max_velocity()))) {
332 if (proposed_.
x > 0.0)
339 && (fabs(proposed_.
y) > fabs(if_colli_target_->
max_velocity()))) {
340 if (proposed_.
y > 0.0)
348 if (proposed_.
rot > 0.0)
This is the base class which calculates drive modes.
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
float get_proposed_trans_x()
Returns the proposed x translation.
float get_proposed_rot()
Returns the proposed rotatio.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
float get_proposed_trans_y()
Returns the proposed y translation.
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
Interface for configuration handling.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Class Escape-Drive-Module.
void set_laser_data(std::vector< polar_coord_2d_t > &laser_points)
This function sets the laser points for one escape round.
Class Escape-Drive-Module.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
Base class for exceptions in Fawkes.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
MotorInterface Fawkes BlackBoard Interface.
float omega() const
Get omega value.
float odometry_position_y() const
Get odometry_position_y value.
float odometry_orientation() const
Get odometry_orientation value.
float odometry_position_x() const
Get odometry_position_x value.
float des_vy() const
Get des_vy value.
float vx() const
Get vx value.
float vy() const
Get vy value.
float des_omega() const
Get des_omega value.
float des_vx() const
Get des_vx value.
NavigatorInterface Fawkes BlackBoard Interface.
float dest_x() const
Get dest_x value.
float dest_y() const
Get dest_y value.
float max_velocity() const
Get max_velocity value.
bool is_stop_at_target() const
Get stop_at_target value.
float max_rotation() const
Get max_rotation value.
DriveMode
Drive modes enum.
@ MovingNotAllowed
Moving not allowed constant.
DriveMode drive_mode() const
Get drive_mode value.
float dest_ori() const
Get dest_ori value.
OrientationMode orientation_mode() const
Get orientation_mode value.
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...
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 get_proposed_trans_y()
Returns the proposed translation. After an update.
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.
void set_local_target(float x, float y)
Set local target point before update!
float get_proposed_trans_x()
Returns the proposed translation. After an update.
void set_local_trajec(float x, float y)
Set local trajectory point before update!
void update(bool escape=false)
Has to be called before the proposed values are called.
Fawkes library namespace.
colli_escape_mode_t
Colli Escape modes.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
float x
Translation in x-direction.
float y
Translation in y-direction.
float rot
Rotation around z-axis.