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