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>
59 NavigatorInterface *target,
61 Configuration * config,
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++) {
217 static_cast<EscapePotentialFieldDriveModule *>(drive_modes_[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++) {
236 EscapeDriveModule *dm = static_cast<EscapeDriveModule *>(drive_modes_[i]);
237 dm->set_laser_data(laser_points);
241 logger_->
log_error(
"SelectDriveMode",
"Can't find escape drive mode to set laser information");
259 AbstractDriveMode *drive_mode = NULL;
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;
310 drive_mode->set_current_robo_speed(if_motor_->
vx(), if_motor_->
vy(), if_motor_->
omega());
312 drive_mode->set_current_target(if_colli_target_->
dest_x(),
313 if_colli_target_->
dest_y(),
316 drive_mode->set_local_target(local_target_.
x, local_target_.
y);
317 drive_mode->set_local_trajec(local_trajec_.
x, local_trajec_.
y);
322 drive_mode->update();
325 proposed_.
x = drive_mode->get_proposed_trans_x();
326 proposed_.
y = drive_mode->get_proposed_trans_y();
327 proposed_.
rot = drive_mode->get_proposed_rot();
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)