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);
ColliThread()
Constructor.
virtual void init()
Initialize the thread.
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
void colli_stop()
Sends a stop-command.
virtual void finalize()
Finalize the thread.
void colli_goto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using global coordinates.
virtual ~ColliThread()
Destructor.
virtual void loop()
Code to execute in the thread.
bool is_final() const
Checks if the colli is final.
virtual void set_vis_thread(ColliVisualizationThread *vis_thread)
Set the visualization thread.
const point_t & get_local_trajec()
return pointer to the local trajectory point.
const point_t & get_local_target()
return pointer to the local target.
void drive(float trans_x, float trans_y, float rot)
Try to realize the proposed values with respect to the maximum allowed values.
void stop()
Executes a soft stop with respect to calculate_translation and calculate_rotation.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
This module is a class for validity checks of drive commands and sets those things with respect to th...
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
const Time * timestamp() const
Get timestamp of last write.
void write()
Write from local copy into BlackBoard memory.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
size_t maxlenof_distances() const
Get maximum length of distances value.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
float update_occ_grid(int mid_x, int mid_y, float inc, float vx, float vy)
Put the laser readings in the occupancy grid.
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
colli_cell_cost_t get_cell_costs() const
Get cell costs.
void reset_old()
Reset all old readings and forget about the world state!
This module is a class for validity checks of drive commands and sets those things with respect to th...
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
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.
Mutex mutual exclusion lock.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
NavigatorInterface Fawkes BlackBoard Interface.
bool is_final() const
Get final value.
float dest_x() const
Get dest_x value.
float dest_y() const
Get dest_y value.
float dest_dist() const
Get dest_dist value.
float max_rotation() const
Get max_rotation value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_dist(const float new_dest_dist)
Set dest_dist value.
void set_dest_y(const float new_dest_y)
Set dest_y value.
virtual void copy_values(const Interface *other)
Copy values from other interface.
bool is_escaping_enabled() const
Get escaping_enabled value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
DriveMode drive_mode() const
Get drive_mode value.
float security_distance() const
Get security_distance value.
float dest_ori() const
Get dest_ori value.
void set_final(const bool new_final)
Set final value.
OrientationMode orientation_mode() const
Get orientation_mode value.
void set_width(int width)
Resets the width of the grid and constructs a new empty grid.
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
void set_height(int height)
Resets the height of the grid and constructs a new empty grid.
int get_cell_width()
Get the cell width (in cm)
int get_height()
Get the height of the grid.
int get_width()
Get the width of the grid.
void set_cell_width(int cell_width)
Resets the cell width (in cm)
void set_cell_height(int cell_height)
Resets the cell height (in cm)
int get_cell_height()
Get the cell height (in cm)
This module is a class for validity checks of drive commands and sets those things with respect to th...
void update(int robo_x, int robo_y, int target_x, int target_y)
update complete plan things
bool updated_successful()
returns, if the update was successful or not.
This class selects the correct drive mode and calls the appopriate drive component.
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 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.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void mark_start()
Mark start of loop.
void wait()
Wait until minimum loop time has been reached.
A class for handling time.
bool is_zero() const
Check if time is zero.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
@ DriveToTarget
Drive to the target.
@ NothingToDo
Indicating that nothing is to do.
@ DriveToOrientPoint
Drive to the orientation point.
@ OrientAtTarget
Indicating that the robot is at target and has to orient.
unsigned int occ
The cost for an occupied cell.
cart_coord_2d_t local_trajec
local trajectory
cart_coord_2d_t local_target
local target
float x
Translation in x-direction.
float y
Translation in y-direction.
float rot
Rotation around z-axis.