24 #include "colli_thread.h"
25 #ifdef HAVE_VISUAL_DEBUGGING
26 # include "visualization_thread.h"
29 #include "drive_modes/select_drive_mode.h"
30 #include "drive_realization/emergency_motor_instruct.h"
31 #include "drive_realization/linear_motor_instruct.h"
32 #include "drive_realization/quadratic_motor_instruct.h"
33 #include "search/astar_search.h"
34 #include "search/og_laser.h"
36 #include <baseapp/run.h>
37 #include <core/threading/mutex.h>
38 #include <interfaces/Laser360Interface.h>
39 #include <interfaces/MotorInterface.h>
40 #include <interfaces/NavigatorInterface.h>
41 #include <tf/time_cache.h>
42 #include <utils/math/common.h>
43 #include <utils/time/wait.h>
71 std::string cfg_prefix =
"/plugins/colli/";
72 frequency_ =
config->
get_int((cfg_prefix +
"frequency").c_str());
73 max_robo_inc_ =
config->
get_float((cfg_prefix +
"max_robo_increase").c_str());
74 cfg_obstacle_inc_ =
config->
get_bool((cfg_prefix +
"obstacle_increasement").c_str());
76 cfg_visualize_idle_ =
config->
get_bool((cfg_prefix +
"visualize_idle").c_str());
79 cfg_min_drive_dist_ =
config->
get_float((cfg_prefix +
"min_drive_distance").c_str());
80 cfg_min_long_dist_drive_ =
config->
get_float((cfg_prefix +
"min_long_dist_drive").c_str());
81 cfg_min_long_dist_prepos_ =
config->
get_float((cfg_prefix +
"min_long_dist_prepos").c_str());
82 cfg_min_rot_dist_ =
config->
get_float((cfg_prefix +
"min_rot_distance").c_str());
83 cfg_target_pre_pos_ =
config->
get_float((cfg_prefix +
"pre_position_distance").c_str());
85 cfg_max_velocity_ =
config->
get_float((cfg_prefix +
"max_velocity").c_str());
90 cfg_iface_motor_ =
config->
get_string((cfg_prefix +
"interface/motor").c_str());
91 cfg_iface_laser_ =
config->
get_string((cfg_prefix +
"interface/laser").c_str());
92 cfg_iface_colli_ =
config->
get_string((cfg_prefix +
"interface/colli").c_str());
93 cfg_iface_read_timeout_ =
config->
get_float((cfg_prefix +
"interface/read_timeout").c_str());
95 cfg_write_spam_debug_ =
config->
get_bool((cfg_prefix +
"write_spam_debug").c_str());
97 cfg_emergency_stop_enabled_ =
98 config->
get_bool((cfg_prefix +
"emergency_stopping/enabled").c_str());
99 cfg_emergency_threshold_distance_ =
100 config->
get_float((cfg_prefix +
"emergency_stopping/threshold_distance").c_str());
101 cfg_emergency_threshold_velocity_ =
102 config->
get_float((cfg_prefix +
"emergency_stopping/threshold_velocity").c_str());
103 cfg_emergency_velocity_max_ =
104 config->
get_float((cfg_prefix +
"emergency_stopping/max_vel").c_str());
106 std::string escape_mode =
config->
get_string((cfg_prefix +
"drive_mode/default_escape").c_str());
107 if (escape_mode.compare(
"potential_field") == 0) {
108 cfg_escape_mode_ = fawkes::colli_escape_mode_t::potential_field;
109 }
else if (escape_mode.compare(
"basic") == 0) {
110 cfg_escape_mode_ = fawkes::colli_escape_mode_t::basic;
112 cfg_escape_mode_ = fawkes::colli_escape_mode_t::basic;
116 std::string motor_instruct_mode =
118 if (motor_instruct_mode.compare(
"linear") == 0) {
119 cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::linear;
120 }
else if (motor_instruct_mode.compare(
"quadratic") == 0) {
121 cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::quadratic;
123 cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::linear;
127 cfg_prefix +=
"occ_grid/";
130 occ_grid_cell_width_ =
config->
get_int((cfg_prefix +
"cell_width").c_str());
131 occ_grid_cell_height_ =
config->
get_int((cfg_prefix +
"cell_height").c_str());
134 distance_to_next_target_ = 1000.f;
140 initialize_modules();
148 #ifdef HAVE_VISUAL_DEBUGGING
149 vis_thread_->setup(occ_grid_, search_);
153 laser_to_base_valid_ =
false;
158 laser_to_base_.
x = p_laser.x();
159 laser_to_base_.
y = p_laser.y();
161 "distance from laser to base: x:%f y:%f",
164 laser_to_base_valid_ =
true;
169 "Unable to transform %s to %s.\n%s",
170 cfg_frame_base_.c_str(),
171 cfg_frame_laser_.c_str(),
179 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
195 delete select_drive_mode_;
198 delete motor_instruct_;
215 vis_thread_ = vis_thread;
224 return colli_data_.
final;
239 colli_goto_(x, y, ori, iface);
262 float colliTargetO = colliCurrentO + ori;
264 this->colli_goto_(colliTargetX, colliTargetY, colliTargetO, iface);
279 if_colli_target_->
write();
289 if (!laser_to_base_valid_) {
295 laser_to_base_.
x = p_laser.x();
296 laser_to_base_.
y = p_laser.y();
298 "distance from laser to base: x:%f y:%f",
301 laser_to_base_valid_ =
true;
306 "Unable to transform %s to %s.\n%s",
307 cfg_frame_base_.c_str(),
308 cfg_frame_laser_.c_str(),
322 if (!interface_data_valid()) {
335 }
else if (if_colli_target_->
drive_mode() == NavigatorInterface::MovingNotAllowed) {
341 }
else if (if_colli_target_->
is_final()) {
348 if (!colli_data_.
final) {
351 if (abs(if_motor_->
vx()) > 0.01f || abs(if_motor_->
vy()) > 0.01f
352 || abs(if_motor_->
omega()) > 0.01f) {
354 motor_instruct_->
stop();
357 colli_data_.
final =
true;
359 motor_instruct_->
stop();
363 #ifdef HAVE_VISUAL_DEBUGGING
364 if (cfg_visualize_idle_) {
366 vis_thread_->wakeup();
376 if_colli_target_->
write();
379 #ifdef HAVE_VISUAL_DEBUGGING
380 vis_thread_->wakeup();
406 float dist = sqrt(x * x + y * y);
410 if_colli_target_->
write();
412 colli_data_.
final =
false;
453 ColliThread::colli_execute_()
456 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
459 update_colli_state();
462 if (colli_state_ == NothingToDo) {
464 if (abs(if_motor_->
vx()) <= 0.01f && abs(if_motor_->
vy()) <= 0.01f
465 && abs(if_motor_->
omega()) <= 0.01f) {
468 colli_data_.
final =
true;
474 #ifdef HAVE_VISUAL_DEBUGGING
475 if (cfg_visualize_idle_)
482 colli_data_.
final =
false;
485 if (check_escape() ==
true || escape_count_ > 0) {
486 if (if_motor_->
des_vx() == 0.f && if_motor_->
des_vy() == 0.f
495 if (escape_count_ > 0)
498 int rnd = (int)((rand()) / (float)(RAND_MAX)) * 10;
500 if (cfg_write_spam_debug_) {
505 if (cfg_write_spam_debug_) {
509 if (cfg_escape_mode_ == fawkes::colli_escape_mode_t::potential_field) {
514 std::vector<polar_coord_2d_t> laser_points;
522 laser_point.
phi = angle_inc * i;
523 laser_points.push_back(laser_point);
527 select_drive_mode_->
update(
true);
534 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
541 if (colli_state_ == OrientAtTarget) {
550 if (proposed_.
rot > 0.f)
552 std::min(if_colli_target_->
max_rotation(), std::max(cfg_min_rot_, proposed_.
rot));
555 std::max(-if_colli_target_->
max_rotation(), std::min(-cfg_min_rot_, proposed_.
rot));
561 search_->
update(robo_grid_pos_.
x,
563 (
int)target_grid_pos_.
x,
564 (
int)target_grid_pos_.
y);
572 (local_grid_target_.
x - robo_grid_pos_.
x) * occ_grid_->
get_cell_width() / 100.f;
574 (local_grid_target_.
y - robo_grid_pos_.
y) * occ_grid_->
get_cell_height() / 100.f;
577 (local_grid_trajec_.
x - robo_grid_pos_.
x) * occ_grid_->
get_cell_width() / 100.f;
579 (local_grid_trajec_.
y - robo_grid_pos_.
y) * occ_grid_->
get_cell_height() / 100.f;
584 select_drive_mode_->
update();
592 local_target_.
x = local_target_.
y = 0.f;
593 local_trajec_.
x = local_trajec_.
y = 0.f;
594 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
604 if (cfg_write_spam_debug_) {
606 name(),
"I want to realize %f , %f , %f", proposed_.
x, proposed_.
y, proposed_.
rot);
610 if (distance_to_next_target_ == 0.f) {
612 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
613 motor_instruct_->
stop();
616 cfg_emergency_stop_enabled_ && distance_to_next_target_ < cfg_emergency_threshold_distance_
617 && if_motor_->
vx() > cfg_emergency_threshold_velocity_) {
618 float max_v = cfg_emergency_velocity_max_;
622 if (!(proposed_.
x == 0.f && proposed_.
y == 0.f)) {
623 part_x = proposed_.
x / ((fabs(proposed_.
x) + fabs(proposed_.
y)));
624 part_y = proposed_.
y / ((fabs(proposed_.
x) + fabs(proposed_.
y)));
627 proposed_.
x = part_x * max_v;
628 proposed_.
y = part_y * max_v;
631 name(),
"Emergency slow down: %f , %f , %f", proposed_.
x, proposed_.
y, proposed_.
rot);
633 emergency_motor_instruct_->
drive(proposed_.
x, proposed_.
y, proposed_.
rot);
637 motor_instruct_->
drive(proposed_.
x, proposed_.
y, proposed_.
rot);
646 ColliThread::open_interfaces()
655 if_colli_target_->
write();
660 ColliThread::initialize_modules()
662 colli_data_.
final =
true;
684 if (cfg_motor_instruct_mode_ == fawkes::colli_motor_instruct_mode_t::linear) {
686 }
else if (cfg_motor_instruct_mode_ == fawkes::colli_motor_instruct_mode_t::quadratic) {
705 delete motor_instruct_;
717 delete motor_instruct_;
718 delete emergency_motor_instruct_;
732 ColliThread::interfaces_read()
740 ColliThread::interface_data_valid()
751 logger->
log_warn(
name(),
"Laser or Motor dead, no writing instance for interfaces!!!");
757 }
else if ((now - if_laser_->
timestamp()) > (
double)cfg_iface_read_timeout_) {
759 "LaserInterface writer has been inactive for too long (%f > %f)",
761 cfg_iface_read_timeout_);
764 }
else if (!colli_data_.
final
765 && (now - if_motor_->
timestamp()) > (
double)cfg_iface_read_timeout_) {
767 "MotorInterface writer has been inactive for too long (%f > %f)",
769 cfg_iface_read_timeout_);
777 "No TimeCache for transform to laser_frame '%s'",
778 cfg_frame_laser_.c_str());
783 if (!cache->get_data(
Time(0, 0), temp)) {
785 "No data in TimeCache for transform to laser frame '%s'",
786 cfg_frame_laser_.c_str());
790 fawkes::Time laser_frame_latest(cache->get_latest_timestamp());
791 if (!laser_frame_latest.is_zero()) {
793 float diff = (now - laser_frame_latest).in_sec();
794 if (diff > 2.f * cfg_iface_read_timeout_) {
796 "Transform to laser frame '%s' is too old (%f > %f)",
797 cfg_frame_laser_.c_str(),
799 2.f * cfg_iface_read_timeout_);
810 ColliThread::update_colli_state()
823 float target_x = if_colli_target_->
dest_x();
824 float target_y = if_colli_target_->
dest_y();
825 float target_ori = if_colli_target_->
dest_ori();
828 == fawkes::NavigatorInterface::OrientationMode::OrientAtTarget
829 && std::isfinite(if_colli_target_->
dest_ori()));
831 float target_dist =
distance(target_x, target_y, cur_x, cur_y);
834 bool is_new_short_target = (if_colli_target_->
dest_dist() < cfg_min_long_dist_drive_)
835 && (if_colli_target_->
dest_dist() >= cfg_min_drive_dist_);
860 if (colli_state_ == OrientAtTarget) {
862 colli_state_ = NothingToDo;
866 if (orient && (target_dist >= cfg_min_long_dist_prepos_)) {
868 float pre_pos_dist = cfg_target_pre_pos_;
869 if (if_motor_->
des_vx() < 0)
870 pre_pos_dist = -pre_pos_dist;
872 target_point_.
x = target_x - (pre_pos_dist * cos(target_ori));
873 target_point_.
y = target_y - (pre_pos_dist * sin(target_ori));
878 }
else if ((target_dist >= cfg_min_long_dist_drive_)
879 || (is_driving && target_dist >= cfg_min_drive_dist_)
880 || (is_new_short_target && target_dist >= cfg_min_drive_dist_)) {
881 target_point_.
x = target_x;
882 target_point_.
y = target_y;
888 >= cfg_min_rot_dist_)) {
904 ColliThread::update_modules()
909 v = std::sqrt(vx * vx + vy * vy);
911 if (!cfg_obstacle_inc_) {
918 occ_grid_->
set_cell_width((
int)std::max((
int)occ_grid_cell_width_, (
int)(5 * fabs(v) + 3)));
919 occ_grid_->
set_cell_height((
int)std::max((
int)occ_grid_cell_height_, (
int)(5 * fabs(v) + 3)));
923 int laserpos_x = (int)(occ_grid_->
get_width() / 2);
924 int laserpos_y = (int)(occ_grid_->
get_height() / 2);
926 laserpos_x -= (int)(vx * occ_grid_->
get_width() / (2 * 3.0));
927 laserpos_x = max(laserpos_x, 10);
928 laserpos_x = min(laserpos_x, (
int)(occ_grid_->
get_width() - 10));
930 int robopos_x = laserpos_x + lround(laser_to_base_.
x * 100 / occ_grid_->
get_cell_width());
931 int robopos_y = laserpos_y + lround(laser_to_base_.
y * 100 / occ_grid_->
get_cell_height());
937 float target_cont_x = (a_x * cos(cur_ori) + a_y * sin(cur_ori));
938 float target_cont_y = (-a_x * sin(cur_ori) + a_y * cos(cur_ori));
941 int target_grid_x = (int)((target_cont_x * 100.f) / (float)occ_grid_->
get_cell_width());
942 int target_grid_y = (int)((target_cont_y * 100.f) / (float)occ_grid_->
get_cell_height());
944 target_grid_x += robopos_x;
945 target_grid_y += robopos_y;
949 if (target_grid_x >= occ_grid_->
get_width() - 1) {
950 target_grid_y = robopos_y
951 + ((robopos_x - (occ_grid_->
get_width() - 2)) / (robopos_x - target_grid_x)
952 * (target_grid_y - robopos_y));
953 target_grid_x = occ_grid_->
get_width() - 2;
956 if (target_grid_x < 2) {
958 robopos_y + ((robopos_x - 2) / (robopos_x - target_grid_x) * (target_grid_y - robopos_y));
962 if (target_grid_y >= occ_grid_->
get_height() - 1) {
963 target_grid_x = robopos_x
964 + ((robopos_y - (occ_grid_->
get_height() - 2)) / (robopos_y - target_grid_y)
965 * (target_grid_x - robopos_x));
969 if (target_grid_y < 2) {
971 robopos_x + ((robopos_y - 2) / (robopos_y - target_grid_y) * (target_grid_x - robopos_x));
976 float robo_inc = 0.f;
981 if (cfg_obstacle_inc_) {
985 float cur_trans = sqrt(if_motor_->
vx() * if_motor_->
vx() + if_motor_->
vy() * if_motor_->
vy());
986 float transinc = max(0.f, cur_trans / 2.f - 0.7f);
987 float rotinc = max(0.f, fabs(if_motor_->
omega() / 3.5f) - 0.7f);
988 float speedinc = max(transinc, rotinc);
991 robo_inc = max(robo_inc, speedinc);
994 robo_inc = min(max_robo_inc_, robo_inc);
998 distance_to_next_target_ = 1000.f;
999 distance_to_next_target_ = occ_grid_->
update_occ_grid(laserpos_x, laserpos_y, robo_inc, vx, vy);
1002 laser_grid_pos_.
x = laserpos_x;
1003 laser_grid_pos_.
y = laserpos_y;
1004 robo_grid_pos_.
x = robopos_x;
1005 robo_grid_pos_.
y = robopos_y;
1006 target_grid_pos_.
x = target_grid_x;
1007 target_grid_pos_.
y = target_grid_y;
1012 ColliThread::check_escape()
1015 return ((
float)occ_grid_->
get_prob(robo_grid_pos_.
x, robo_grid_pos_.
y) == cell_cost_occ);