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/og_laser.h" 34 #include "search/astar_search.h" 36 #include <core/threading/mutex.h> 37 #include <baseapp/run.h> 38 #include <utils/time/wait.h> 39 #include <interfaces/MotorInterface.h> 40 #include <interfaces/Laser360Interface.h> 41 #include <interfaces/NavigatorInterface.h> 42 #include <utils/math/common.h> 43 #include <tf/time_cache.h> 73 std::string cfg_prefix =
"/plugins/colli/";
74 frequency_ =
config->
get_int((cfg_prefix +
"frequency").c_str());
75 max_robo_inc_ =
config->
get_float((cfg_prefix +
"max_robo_increase").c_str());
76 cfg_obstacle_inc_ =
config->
get_bool((cfg_prefix +
"obstacle_increasement").c_str());
78 cfg_visualize_idle_ =
config->
get_bool((cfg_prefix +
"visualize_idle").c_str());
81 cfg_min_drive_dist_ =
config->
get_float((cfg_prefix +
"min_drive_distance").c_str());
82 cfg_min_long_dist_drive_ =
config->
get_float((cfg_prefix +
"min_long_dist_drive").c_str());
83 cfg_min_long_dist_prepos_ =
config->
get_float((cfg_prefix +
"min_long_dist_prepos").c_str());
84 cfg_min_rot_dist_ =
config->
get_float((cfg_prefix +
"min_rot_distance").c_str());
85 cfg_target_pre_pos_ =
config->
get_float((cfg_prefix +
"pre_position_distance").c_str());
87 cfg_max_velocity_ =
config->
get_float((cfg_prefix +
"max_velocity").c_str());
92 cfg_iface_motor_ =
config->
get_string((cfg_prefix +
"interface/motor").c_str());
93 cfg_iface_laser_ =
config->
get_string((cfg_prefix +
"interface/laser").c_str());
94 cfg_iface_colli_ =
config->
get_string((cfg_prefix +
"interface/colli").c_str());
95 cfg_iface_read_timeout_ =
config->
get_float((cfg_prefix +
"interface/read_timeout").c_str());
97 cfg_write_spam_debug_ =
config->
get_bool((cfg_prefix +
"write_spam_debug").c_str());
99 cfg_emergency_stop_enabled_ =
config->
get_bool((cfg_prefix +
"emergency_stopping/enabled").c_str());
100 cfg_emergency_threshold_distance_ =
config->
get_float((cfg_prefix +
"emergency_stopping/threshold_distance").c_str());
101 cfg_emergency_threshold_velocity_ =
config->
get_float((cfg_prefix +
"emergency_stopping/threshold_velocity").c_str());
102 cfg_emergency_velocity_max_ =
config->
get_float((cfg_prefix +
"emergency_stopping/max_vel").c_str());
104 std::string escape_mode =
config->
get_string((cfg_prefix +
"drive_mode/default_escape").c_str());
105 if ( escape_mode.compare(
"potential_field") == 0 ) {
106 cfg_escape_mode_ = fawkes::colli_escape_mode_t::potential_field;
107 }
else if ( escape_mode.compare(
"basic") == 0 ) {
108 cfg_escape_mode_ = fawkes::colli_escape_mode_t::basic;
110 cfg_escape_mode_ = fawkes::colli_escape_mode_t::basic;
114 std::string motor_instruct_mode =
config->
get_string((cfg_prefix +
"motor_instruct/mode").c_str());
115 if ( motor_instruct_mode.compare(
"linear") == 0 ) {
116 cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::linear;
117 }
else if ( motor_instruct_mode.compare(
"quadratic") == 0 ) {
118 cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::quadratic;
120 cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::linear;
124 cfg_prefix +=
"occ_grid/";
127 occ_grid_cell_width_ =
config->
get_int((cfg_prefix +
"cell_width").c_str());
128 occ_grid_cell_height_ =
config->
get_int((cfg_prefix +
"cell_height").c_str());
130 srand( time( NULL ) );
131 distance_to_next_target_ = 1000.f;
137 initialize_modules();
145 #ifdef HAVE_VISUAL_DEBUGGING 146 vis_thread_->setup(occ_grid_, search_);
150 laser_to_base_valid_ =
false;
155 laser_to_base_.
x = p_laser.x();
156 laser_to_base_.
y = p_laser.y();
158 laser_to_base_.
x, laser_to_base_.
y);
159 laser_to_base_valid_ =
true;
164 cfg_frame_base_.c_str(), cfg_frame_laser_.c_str(), e.
what() );
171 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
188 delete select_drive_mode_;
191 delete motor_instruct_;
208 vis_thread_ = vis_thread;
217 return colli_data_.
final;
232 colli_goto_(x, y, ori, iface);
252 + x * cos( colliCurrentO )
253 - y * sin( colliCurrentO );
255 + x * sin( colliCurrentO )
256 + y * cos( colliCurrentO );
257 float colliTargetO = colliCurrentO + ori;
259 this->colli_goto_(colliTargetX, colliTargetY, colliTargetO, iface);
274 if_colli_target_->
write();
284 if( !laser_to_base_valid_ ) {
290 laser_to_base_.
x = p_laser.x();
291 laser_to_base_.
y = p_laser.y();
293 laser_to_base_.
x, laser_to_base_.
y);
294 laser_to_base_valid_ =
true;
299 cfg_frame_base_.c_str(), cfg_frame_laser_.c_str(),
313 if( !interface_data_valid() ) {
326 }
else if( if_colli_target_->
drive_mode() == NavigatorInterface::MovingNotAllowed ) {
332 }
else if( if_colli_target_->
is_final() ) {
339 if( !colli_data_.
final ) {
342 if( abs(if_motor_->
vx()) > 0.01f
343 || abs(if_motor_->
vy()) > 0.01f
344 || abs(if_motor_->
omega()) > 0.01f ) {
346 motor_instruct_->
stop();
349 colli_data_.
final =
true;
351 motor_instruct_->
stop();
355 #ifdef HAVE_VISUAL_DEBUGGING 356 if( cfg_visualize_idle_ ) {
358 vis_thread_->wakeup();
369 if_colli_target_->
write();
372 #ifdef HAVE_VISUAL_DEBUGGING 373 vis_thread_->wakeup();
399 float dist = sqrt(x*x + y*y);
403 if_colli_target_->
write();
405 colli_data_.
final =
false;
446 ColliThread::colli_execute_()
449 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
452 update_colli_state();
457 if( abs(if_motor_->
vx()) <= 0.01f
458 && abs(if_motor_->
vy()) <= 0.01f
459 && abs(if_motor_->
omega()) <= 0.01f ) {
462 colli_data_.
final =
true;
468 #ifdef HAVE_VISUAL_DEBUGGING 469 if( cfg_visualize_idle_ )
476 colli_data_.
final =
false;
479 if( check_escape() ==
true || escape_count_ > 0 ) {
480 if( if_motor_->
des_vx() == 0.f
481 && if_motor_->
des_vy() == 0.f
490 if (escape_count_ > 0)
493 int rnd = (int)((rand())/(
float)(RAND_MAX)) * 10;
495 if (cfg_write_spam_debug_) {
500 if (cfg_write_spam_debug_) {
504 if ( cfg_escape_mode_ == fawkes::colli_escape_mode_t::potential_field ) {
509 std::vector<polar_coord_2d_t> laser_points;
517 laser_point.
phi = angle_inc * i;
518 laser_points.push_back(laser_point);
522 select_drive_mode_->
update(
true );
529 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
544 if ( proposed_.
rot > 0.f )
545 proposed_.
rot = std::min( if_colli_target_->
max_rotation(), std::max( cfg_min_rot_, proposed_.
rot));
547 proposed_.
rot = std::max(-if_colli_target_->
max_rotation(), std::min(-cfg_min_rot_, proposed_.
rot));
553 search_->
update( robo_grid_pos_.
x, robo_grid_pos_.
y,
554 (
int)target_grid_pos_.
x, (
int)target_grid_pos_.
y );
561 local_target_.
x = (local_grid_target_.
x - robo_grid_pos_.
x)*occ_grid_->
get_cell_width()/100.f;
562 local_target_.
y = (local_grid_target_.
y - robo_grid_pos_.
y)*occ_grid_->
get_cell_height()/100.f;
564 local_trajec_.
x = (local_grid_trajec_.
x - robo_grid_pos_.
x)*occ_grid_->
get_cell_width()/100.f;
565 local_trajec_.
y = (local_grid_trajec_.
y - robo_grid_pos_.
y)*occ_grid_->
get_cell_height()/100.f;
570 select_drive_mode_->
update();
578 local_target_.
x = local_target_.
y = 0.f;
579 local_trajec_.
x = local_trajec_.
y = 0.f;
580 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
592 if (cfg_write_spam_debug_) {
597 if( distance_to_next_target_ == 0.f ) {
599 proposed_.
x = proposed_.
y = proposed_.
rot = 0.f;
600 motor_instruct_->
stop();
603 cfg_emergency_stop_enabled_
604 && distance_to_next_target_ < cfg_emergency_threshold_distance_
605 && if_motor_->
vx() > cfg_emergency_threshold_velocity_ ) {
606 float max_v = cfg_emergency_velocity_max_;
610 if ( ! (proposed_.
x == 0.f && proposed_.
y == 0.f) ) {
611 part_x = proposed_.
x / ( ( fabs(proposed_.
x) + fabs(proposed_.
y) ) );
612 part_y = proposed_.
y / ( ( fabs(proposed_.
x) + fabs(proposed_.
y) ) );
615 proposed_.
x = part_x * max_v;
616 proposed_.
y = part_y * max_v;
620 emergency_motor_instruct_->
drive( proposed_.
x, proposed_.
y, proposed_.
rot );
624 motor_instruct_->
drive( proposed_.
x, proposed_.
y, proposed_.
rot );
634 ColliThread::open_interfaces()
643 if_colli_target_->
write();
649 ColliThread::initialize_modules()
651 colli_data_.
final =
true;
673 if ( cfg_motor_instruct_mode_ == fawkes::colli_motor_instruct_mode_t::linear ) {
678 }
else if ( cfg_motor_instruct_mode_ == fawkes::colli_motor_instruct_mode_t::quadratic ) {
706 delete motor_instruct_;
717 delete motor_instruct_;
718 delete emergency_motor_instruct_;
735 ColliThread::interfaces_read()
744 ColliThread::interface_data_valid()
755 logger->
log_warn(
name(),
"Laser or Motor dead, no writing instance for interfaces!!!");
761 }
else if( (now - if_laser_->
timestamp()) > (
double)cfg_iface_read_timeout_ ) {
762 logger->
log_warn(
name(),
"LaserInterface writer has been inactive for too long (%f > %f)",
763 (now - if_laser_->
timestamp()), cfg_iface_read_timeout_);
766 }
else if( !colli_data_.
final && (now - if_motor_->
timestamp()) > (
double)cfg_iface_read_timeout_ ) {
767 logger->
log_warn(
name(),
"MotorInterface writer has been inactive for too long (%f > %f)",
768 (now - if_motor_->
timestamp()), cfg_iface_read_timeout_);
775 logger->
log_warn(
name(),
"No TimeCache for transform to laser_frame '%s'", cfg_frame_laser_.c_str());
780 if( !cache->get_data(
Time(0,0), temp)) {
781 logger->
log_warn(
name(),
"No data in TimeCache for transform to laser frame '%s'", cfg_frame_laser_.c_str());
785 fawkes::Time laser_frame_latest(cache->get_latest_timestamp());
786 if (! laser_frame_latest.is_zero()) {
788 float diff = (now - laser_frame_latest).in_sec();
789 if( diff > 2.f* cfg_iface_read_timeout_) {
791 cfg_frame_laser_.c_str(), diff, 2.f*cfg_iface_read_timeout_);
804 ColliThread::update_colli_state()
817 float target_x = if_colli_target_->
dest_x();
818 float target_y = if_colli_target_->
dest_y();
819 float target_ori = if_colli_target_->
dest_ori();
821 bool orient = ( if_colli_target_->
orientation_mode() == fawkes::NavigatorInterface::OrientationMode::OrientAtTarget
822 && std::isfinite(if_colli_target_->
dest_ori()) );
824 float target_dist =
distance(target_x, target_y, cur_x, cur_y);
827 bool is_new_short_target = (if_colli_target_->
dest_dist() < cfg_min_long_dist_drive_)
828 && (if_colli_target_->
dest_dist() >= cfg_min_drive_dist_);
859 if( orient && ( target_dist >= cfg_min_long_dist_prepos_ ) ) {
861 float pre_pos_dist = cfg_target_pre_pos_;
862 if ( if_motor_->
des_vx() < 0 )
863 pre_pos_dist = -pre_pos_dist;
865 target_point_.
x = target_x - ( pre_pos_dist * cos(target_ori) );
866 target_point_.
y = target_y - ( pre_pos_dist * sin(target_ori) );
871 }
else if( (target_dist >= cfg_min_long_dist_drive_)
872 || (is_driving && target_dist >= cfg_min_drive_dist_)
873 || (is_new_short_target && target_dist >= cfg_min_drive_dist_) ) {
874 target_point_.
x = target_x;
875 target_point_.
y = target_y;
879 }
else if ( orient && ( fabs(
normalize_mirror_rad(cur_ori - target_ori) ) >= cfg_min_rot_dist_ ) ) {
897 ColliThread::update_modules()
902 v = std::sqrt( vx*vx + vy*vy );
904 if ( !cfg_obstacle_inc_ ) {
911 occ_grid_->
set_cell_width( (
int)std::max( (
int)occ_grid_cell_width_,
912 (
int)(5*fabs( v )+3) ) );
914 (
int)(5*fabs( v )+3) ) );
918 int laserpos_x = (int)(occ_grid_->
get_width() / 2);
919 int laserpos_y = (int)(occ_grid_->
get_height() / 2);
921 laserpos_x -= (int)( vx * occ_grid_->
get_width() / (2*3.0) );
922 laserpos_x = max ( laserpos_x, 10 );
923 laserpos_x = min ( laserpos_x, (
int)(occ_grid_->
get_width()-10) );
925 int robopos_x = laserpos_x + lround( laser_to_base_.
x*100 / occ_grid_->
get_cell_width() );
926 int robopos_y = laserpos_y + lround( laser_to_base_.
y*100 / occ_grid_->
get_cell_height() );
932 float target_cont_x = ( a_x*cos( cur_ori ) + a_y*sin( cur_ori ) );
933 float target_cont_y = (-a_x*sin( cur_ori ) + a_y*cos( cur_ori ) );
936 int target_grid_x = (int)( (target_cont_x * 100.f) / (float)occ_grid_->
get_cell_width() );
937 int target_grid_y = (int)( (target_cont_y * 100.f) / (float)occ_grid_->
get_cell_height() );
939 target_grid_x += robopos_x;
940 target_grid_y += robopos_y;
945 if (target_grid_x >= occ_grid_->
get_width()-1) {
946 target_grid_y = robopos_y + ((robopos_x - (occ_grid_->
get_width()-2))/(robopos_x - target_grid_x) * (target_grid_y - robopos_y));
947 target_grid_x = occ_grid_->
get_width()-2;
950 if (target_grid_x < 2) {
951 target_grid_y = robopos_y + ((robopos_x-2)/(robopos_x - target_grid_x) * (target_grid_y - robopos_y));
955 if (target_grid_y >= occ_grid_->
get_height()-1) {
956 target_grid_x = robopos_x + ((robopos_y - (occ_grid_->
get_height()-2))/(robopos_y - target_grid_y) * (target_grid_x - robopos_x));
960 if (target_grid_y < 2) {
961 target_grid_x = robopos_x + ((robopos_y-2)/(robopos_y - target_grid_y) * (target_grid_x - robopos_x));
966 float robo_inc = 0.f;
971 if ( cfg_obstacle_inc_ ) {
975 float cur_trans = sqrt(if_motor_->
vx()*if_motor_->
vx() + if_motor_->
vy()*if_motor_->
vy());
976 float transinc = max(0.f,cur_trans/2.f -0.7f);
977 float rotinc = max(0.f,fabs( if_motor_->
omega()/3.5f )-0.7f);
978 float speedinc = max( transinc, rotinc );
981 robo_inc = max( robo_inc, speedinc);
984 robo_inc = min( max_robo_inc_, robo_inc );
988 distance_to_next_target_ = 1000.f;
989 distance_to_next_target_ = occ_grid_->
update_occ_grid( laserpos_x, laserpos_y, robo_inc, vx, vy );
992 laser_grid_pos_.
x = laserpos_x;
993 laser_grid_pos_.
y = laserpos_y;
994 robo_grid_pos_.
x = robopos_x;
995 robo_grid_pos_.
y = robopos_y;
996 target_grid_pos_.
x = target_grid_x;
997 target_grid_pos_.
y = target_grid_y;
1002 ColliThread::check_escape()
1005 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.
The Basic of a Motorinstructor.
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.