Fawkes API
Fawkes Development Version
|
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)
float dest_y() const
Get dest_y value.
float des_vx() const
Get des_vx value.
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
float vx() const
Get vx value.
float get_proposed_trans_y()
Returns the proposed translation. After an update.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
float omega() const
Get omega value.
float rot
Rotation around z-axis.
void set_local_trajec(float x, float y)
Set local trajectory point before update!
float get_proposed_rot()
Returns the proposed rotatio.
float dest_x() const
Get dest_x value.
float y
Translation in y-direction.
float des_vy() const
Get des_vy value.
~SelectDriveMode()
Desctructor.
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...
MotorInterface Fawkes BlackBoard Interface.
Class Escape-Drive-Module.
@ MovingNotAllowed
Moving not allowed constant.
float odometry_position_y() const
Get odometry_position_y value.
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...
Interface for configuration handling.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
colli_escape_mode_t
Colli Escape modes.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
This is the base class which calculates drive modes.
float x
Translation in x-direction.
float des_omega() const
Get des_omega value.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
DriveMode
Drive modes enum.
Fawkes library namespace.
float odometry_orientation() const
Get odometry_orientation value.
void set_local_target(float x, float y)
Set local target point before update!
float max_velocity() const
Get max_velocity value.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
float max_rotation() const
Get max_rotation value.
float get_proposed_trans_y()
Returns the proposed y translation.
float get_proposed_trans_x()
Returns the proposed translation. After an update.
float dest_ori() const
Get dest_ori value.
void set_laser_data(std::vector< polar_coord_2d_t > &laser_points)
This function sets the laser points for one escape round.
bool is_stop_at_target() const
Get stop_at_target value.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
float get_proposed_trans_x()
Returns the proposed x translation.
void set_current_target(float x, float y, float ori)
Sets the current target.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Class Escape-Drive-Module.
float odometry_position_x() const
Get odometry_position_x value.
float get_proposed_rot()
Returns the proposed rotation. After an update.
SelectDriveMode(MotorInterface *motor, NavigatorInterface *colli_target, Logger *logger, Configuration *config, colli_escape_mode_t escape_mode=colli_escape_mode_t::basic)
Constructor.
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.
NavigatorInterface Fawkes BlackBoard Interface.
float vy() const
Get vy value.
OrientationMode orientation_mode() const
Get orientation_mode value.
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
void update(bool escape=false)
Has to be called before the proposed values are called.
DriveMode drive_mode() const
Get drive_mode value.
Base class for exceptions in Fawkes.