Fawkes API  Fawkes Development Version
colli_thread.cpp
1 
2 /***************************************************************************
3  * colli_thread.cpp - Fawkes Colli Thread
4  *
5  * Created: Sat Jul 13 12:00:00 2013
6  * Copyright 2013-2014 Bahram Maleki-Fard
7  * 2014 Tobias Neumann
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "colli_thread.h"
25 #ifdef HAVE_VISUAL_DEBUGGING
26  #include "visualization_thread.h"
27 #endif
28 
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"
35 
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>
44 
45 #include <string>
46 
47 using namespace fawkes;
48 using namespace std;
49 
50 /** @class ColliThread "colli_thread.h"
51  * Thread that performs the navigation and collision avoidance algorithms.
52  */
53 
54 /** Constructor. */
56  : Thread("ColliThread", Thread::OPMODE_CONTINUOUS),
57  vis_thread_( 0 )
58 {
59  mutex_ = new Mutex();
60 }
61 
62 /** Destructor. */
64 {
65  delete mutex_;
66 }
67 
68 void
70 {
71  logger->log_debug(name(), "(init): Constructing...");
72 
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());
77 
78  cfg_visualize_idle_ = config->get_bool((cfg_prefix + "visualize_idle").c_str());
79 
80  cfg_min_rot_ = config->get_float((cfg_prefix + "min_rot").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());
86 
87  cfg_max_velocity_ = config->get_float((cfg_prefix + "max_velocity").c_str());
88 
89  cfg_frame_base_ = config->get_string((cfg_prefix + "frame/base").c_str());
90  cfg_frame_laser_ = config->get_string((cfg_prefix + "frame/laser").c_str());
91 
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());
96 
97  cfg_write_spam_debug_ = config->get_bool((cfg_prefix + "write_spam_debug").c_str());
98 
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());
103 
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;
109  } else {
110  cfg_escape_mode_ = fawkes::colli_escape_mode_t::basic;
111  throw fawkes::Exception("Default escape drive_mode is unknown");
112  }
113 
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;
119  } else {
120  cfg_motor_instruct_mode_ = fawkes::colli_motor_instruct_mode_t::linear;
121  throw fawkes::Exception("Motor instruct mode is unknown, use linear");
122  }
123 
124  cfg_prefix += "occ_grid/";
125  occ_grid_width_ = config->get_float((cfg_prefix + "width").c_str());
126  occ_grid_height_ = config->get_float((cfg_prefix + "height").c_str());
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());
129 
130  srand( time( NULL ) );
131  distance_to_next_target_ = 1000.f;
132 
133  logger->log_debug(name(), "(init): Entering initialization ..." );
134 
135  open_interfaces();
136  try {
137  initialize_modules();
138  } catch(Exception &e) {
139  blackboard->close( if_colli_target_ );
140  blackboard->close( if_laser_ );
141  blackboard->close( if_motor_ );
142  throw;
143  }
144 
145 #ifdef HAVE_VISUAL_DEBUGGING
146  vis_thread_->setup(occ_grid_, search_);
147 #endif
148 
149  // get distance from laser to robot base
150  laser_to_base_valid_ = false;
151  tf::Stamped<tf::Point> p_laser;
152  tf::Stamped<tf::Point> p_base( tf::Point(0,0,0), fawkes::Time(0,0), cfg_frame_base_);
153  try {
154  tf_listener->transform_point(cfg_frame_laser_, p_base, p_laser);
155  laser_to_base_.x = p_laser.x();
156  laser_to_base_.y = p_laser.y();
157  logger->log_info(name(), "distance from laser to base: x:%f y:%f",
158  laser_to_base_.x, laser_to_base_.y);
159  laser_to_base_valid_ = true;
160  occ_grid_->set_base_offset(laser_to_base_.x, laser_to_base_.y);
161  } catch(Exception &e) {
162  if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
163  logger->log_warn(name(), "Unable to transform %s to %s.\n%s",
164  cfg_frame_base_.c_str(), cfg_frame_laser_.c_str(), e.what() );
165  }
166  }
167 
168  // setup timer for colli-frequency
169  timer_ = new TimeWait(clock, 1e6 / frequency_);
170 
171  proposed_.x = proposed_.y = proposed_.rot = 0.f;
172 
173  target_new_ = false;
174  escape_count_ = 0;
175 
176  logger->log_debug(name(), "(init): Initialization done.");
177 }
178 
179 
180 void
182 {
183  logger->log_debug(name(), "(finalize): Entering destructing ...");
184 
185  delete timer_;
186 
187  // delete own modules
188  delete select_drive_mode_;
189  delete search_;
190  delete occ_grid_;
191  delete motor_instruct_;
192 
193  // close all registered bb-interfaces
194  blackboard->close( if_colli_target_ );
195  blackboard->close( if_laser_ );
196  blackboard->close( if_motor_ );
197 
198  logger->log_debug(name(), "(finalize): Destructing done.");
199 }
200 
201 /** Set the visualization thread.
202  * By default, it is created by the plugin (colli_plugin.cpp) and passed to the colli_thread.
203  * @param vis_thread Pointer to the visualization-thread
204  */
205 void
206 ColliThread::set_vis_thread(ColliVisualizationThread* vis_thread)
207 {
208  vis_thread_ = vis_thread;
209 }
210 
211 /** Checks if the colli is final.
212  * @return True if colli is final, false otherwise.
213  */
214 bool
216 {
217  return colli_data_.final;
218 }
219 
220 /** Sends a goto-command, using global coordinates.
221  * @param x Global x-coordinate of destination
222  * @param y Global y-coordinate of destination
223  * @param ori Global orientation of robot at destination
224  * @param iface This interface holds the colli-parameters for the new destination
225  */
226 void
227 ColliThread::colli_goto(float x, float y, float ori, NavigatorInterface* iface)
228 {
229  mutex_->lock();
230  interfaces_read();
231 
232  colli_goto_(x, y, ori, iface);
233 }
234 
235 /** Sends a goto-command, using relative coordinates.
236  * @param x Relative x-coordinate of destination
237  * @param y Relative y-coordinate of destination
238  * @param ori Relative orientation of robot at destination
239  * @param iface This interface holds the colli-parameters for the new destination
240  */
241 void
242 ColliThread::colli_relgoto(float x, float y, float ori, NavigatorInterface* iface)
243 {
244  mutex_->lock();
245  interfaces_read();
246 
247  float colliCurrentO = if_motor_->odometry_orientation();
248 
249  //TODO: use TF instead tranform from base_link to odom
250  // coord transformation: relative target -> (global) motor coordinates
251  float colliTargetX = if_motor_->odometry_position_x()
252  + x * cos( colliCurrentO )
253  - y * sin( colliCurrentO );
254  float colliTargetY = if_motor_->odometry_position_y()
255  + x * sin( colliCurrentO )
256  + y * cos( colliCurrentO );
257  float colliTargetO = colliCurrentO + ori;
258 
259  this->colli_goto_(colliTargetX, colliTargetY, colliTargetO, iface);
260 }
261 
262 /** Sends a stop-command.
263  * Colli will stop and discard the previous destinations. */
264 void
266 {
267  mutex_->lock();
268 
269  if_colli_target_->set_dest_x( if_motor_->odometry_position_x() );
270  if_colli_target_->set_dest_y( if_motor_->odometry_position_y() );
271  if_colli_target_->set_dest_ori( if_motor_->odometry_orientation() );
272  if_colli_target_->set_dest_dist( 0.f );
273  if_colli_target_->set_final( true );
274  if_colli_target_->write();
275  mutex_->unlock();
276 }
277 
278 void
280 {
281  timer_->mark_start();
282 
283  // Do not continue if we don't have a valid transform from base to laser yet
284  if( !laser_to_base_valid_ ) {
285  try {
286  tf::Stamped<tf::Point> p_laser;
287  tf::Stamped<tf::Point> p_base( tf::Point(0,0,0), fawkes::Time(0,0), cfg_frame_base_);
288 
289  tf_listener->transform_point(cfg_frame_laser_, p_base, p_laser);
290  laser_to_base_.x = p_laser.x();
291  laser_to_base_.y = p_laser.y();
292  logger->log_info(name(), "distance from laser to base: x:%f y:%f",
293  laser_to_base_.x, laser_to_base_.y);
294  laser_to_base_valid_ = true;
295  occ_grid_->set_base_offset(laser_to_base_.x, laser_to_base_.y);
296  } catch(Exception &e) {
297  if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
298  logger->log_warn(name(), "Unable to transform %s to %s.\n%s",
299  cfg_frame_base_.c_str(), cfg_frame_laser_.c_str(),
300  e.what_no_backtrace());
301  }
302  timer_->wait();
303  return;
304  }
305  }
306 
307  mutex_->lock();
308 
309  interfaces_read();
310 
311  // check if we need to abort for some reason
312  bool abort = false;
313  if( !interface_data_valid() ) {
314  escape_count_ = 0;
315  abort = true;
316 
317 /*
318  // THIS IF FOR CHALLENGE ONLY!!!
319  } else if( if_colli_target_->drive_mode() == NavigatorInterface::OVERRIDE ) {
320  logger->log_debug(name(), "BEING OVERRIDDEN!");
321  colli_data_.final = false;
322  escape_count_ = 0;
323  abort = true;
324 */
325 
326  } else if( if_colli_target_->drive_mode() == NavigatorInterface::MovingNotAllowed ) {
327  //logger->log_debug(name(), "Moving is not allowed!");
328  escape_count_ = 0;
329  abort = true;
330 
331  // Do not drive if there is no new target
332  } else if( if_colli_target_->is_final() ) {
333  //logger->log_debug(name(), "No new target for colli...ABORT");
334  abort = true;
335  }
336 
337  if( abort ) {
338  // check if we need to stop the current colli movement
339  if( !colli_data_.final ) {
340  //logger->log_debug(name(), "STOPPING");
341  // colli is active, but for some reason we need to abort -> STOP colli movement
342  if( abs(if_motor_->vx()) > 0.01f
343  || abs(if_motor_->vy()) > 0.01f
344  || abs(if_motor_->omega()) > 0.01f ) {
345  // only stop movement, if we are moving
346  motor_instruct_->stop();
347  } else {
348  // movement has stopped, we are "final" now
349  colli_data_.final = true;
350  // send one final stop, just to make sure we really stop
351  motor_instruct_->stop();
352  }
353  }
354 
355 #ifdef HAVE_VISUAL_DEBUGGING
356  if( cfg_visualize_idle_ ) {
357  update_modules();
358  vis_thread_->wakeup();
359  }
360 #endif
361 
362  } else {
363 
364  // Run Colli
365  colli_execute_();
366 
367  // Send motor and colli data away.
368  if_colli_target_->set_final( colli_data_.final );
369  if_colli_target_->write();
370 
371  // visualize the new information
372 #ifdef HAVE_VISUAL_DEBUGGING
373  vis_thread_->wakeup();
374 #endif
375  }
376 
377  mutex_->unlock();
378 
379  timer_->wait();
380 }
381 
382 /* **************************************************************************** */
383 /* **************************************************************************** */
384 /* ****************** P R I V A T E - S T U F F **************************** */
385 /* **************************************************************************** */
386 /* **************************************************************************** */
387 void
388 ColliThread::colli_goto_(float x, float y, float ori, NavigatorInterface* iface)
389 {
390  if_colli_target_->copy_values(iface);
391 
392  if_colli_target_->set_dest_x( x );
393  if_colli_target_->set_dest_y( y );
394  if_colli_target_->set_dest_ori( ori );
395 
396  // x and y are not needed anymore. use them for calculation of target distance
397  x -= if_motor_->odometry_position_x();
398  y -= if_motor_->odometry_position_y();
399  float dist = sqrt(x*x + y*y);
400  if_colli_target_->set_dest_dist(dist);
401 
402  if_colli_target_->set_final( false );
403  if_colli_target_->write();
404 
405  colli_data_.final = false;
406  target_new_ = true;
407  mutex_->unlock();
408 }
409 
410 // ============================================================================ //
411 // ============================================================================ //
412 // BBCLIENT LOOP //
413 // ************* //
414 // //
415 // The desired structure should be something like this //
416 // =================================================== //
417 // //
418 // update the state machine //
419 // //
420 // If we are in stop state //
421 // Do stop //
422 // Else if we are in orient state //
423 // Do orient //
424 // else if we are in a drive state //
425 // update the grid //
426 // If we are to close to an obstacle //
427 // Escape the obstacle //
428 // Get Motor settings for escaping //
429 // Set Motor parameters for escaping //
430 // else //
431 // search for a way //
432 // if we found a way, //
433 // Translate the way in motor things //
434 // Set Motor parameters for driving //
435 // else //
436 // do nothing, because this is an error! //
437 // Set Motor parameters for stopping //
438 // //
439 // Translate and Realize the motor commands //
440 // update the BB Things //
441 // //
442 // ============================================================================ //
443 // ============================================================================ //
444 //
445 void
446 ColliThread::colli_execute_()
447 {
448  // to be on the sure side of life
449  proposed_.x = proposed_.y = proposed_.rot = 0.f;
450 
451  // update state machine
452  update_colli_state();
453 
454  // nothing is to do
455  if (colli_state_ == NothingToDo) {
456  occ_grid_->reset_old();
457  if( abs(if_motor_->vx()) <= 0.01f
458  && abs(if_motor_->vy()) <= 0.01f
459  && abs(if_motor_->omega()) <= 0.01f ) {
460  // we have stopped, can consider the colli final now
461  //logger->log_debug(name(), "L, consider colli final now");
462  colli_data_.final = true;
463  }
464 
465  occ_grid_->reset_old();
466  escape_count_ = 0;
467 
468 #ifdef HAVE_VISUAL_DEBUGGING
469  if( cfg_visualize_idle_ )
470  update_modules();
471 #endif
472 
473  } else {
474  // perform the update of the grid.
475  update_modules();
476  colli_data_.final = false;
477 
478  // Check, if one of our positions (robo-, laser-gridpos is not valid) => Danger!
479  if( check_escape() == true || escape_count_ > 0 ) {
480  if( if_motor_->des_vx() == 0.f
481  && if_motor_->des_vy() == 0.f
482  && if_motor_->des_omega() == 0.f ) {
483  occ_grid_->reset_old();
484  }
485 
486  // ueber denken und testen
487 
488  if( if_colli_target_->is_escaping_enabled() ) {
489  // SJTODO: ERST wenn ich gestoppt habe, escape mode anwerfen!!!
490  if (escape_count_ > 0)
491  escape_count_--;
492  else {
493  int rnd = (int)((rand())/(float)(RAND_MAX)) * 10; // + 5;
494  escape_count_ = rnd;
495  if (cfg_write_spam_debug_) {
496  logger->log_debug(name(), "Escape: new round with %i", rnd);
497  }
498  }
499 
500  if (cfg_write_spam_debug_) {
501  logger->log_debug(name(), "Escape mode, escaping!");
502  }
503  select_drive_mode_->set_local_target( local_target_.x, local_target_.y );
504  if ( cfg_escape_mode_ == fawkes::colli_escape_mode_t::potential_field ) {
505  select_drive_mode_->set_grid_information(occ_grid_, robo_grid_pos_.x, robo_grid_pos_.y);
506  } else {
507  if_laser_->read();
508 
509  std::vector<polar_coord_2d_t> laser_points;
510  laser_points.reserve(if_laser_->maxlenof_distances());
511 
512  float angle_inc = 2.f * M_PI / if_laser_->maxlenof_distances();
513 
514  polar_coord_2d_t laser_point;
515  for ( unsigned int i = 0; i < if_laser_->maxlenof_distances(); ++i ) {
516  laser_point.r = if_laser_->distances(i);
517  laser_point.phi = angle_inc * i;
518  laser_points.push_back(laser_point);
519  }
520  select_drive_mode_->set_laser_data(laser_points);
521  }
522  select_drive_mode_->update( true ); // <-- this calls the ESCAPE mode!
523  proposed_.x = select_drive_mode_->get_proposed_trans_x();
524  proposed_.y = select_drive_mode_->get_proposed_trans_y();
525  proposed_.rot = select_drive_mode_->get_proposed_rot();
526 
527  } else {
528  logger->log_warn(name(), "Escape mode, but not allowed!");
529  proposed_.x = proposed_.y = proposed_.rot = 0.f;
530  escape_count_ = 0;
531  }
532 
533  } else {
534  // only orienting to do and moving possible
535 
536  if (colli_state_ == OrientAtTarget) {
537  proposed_.x = 0.f;
538  proposed_.y = 0.f;
539  // turn faster if angle-diff is high
540  //proposed_.rot = 1.5*normalize_mirror_rad( if_colli_target_->GetTargetOri() -
541  proposed_.rot = 1.f*normalize_mirror_rad( if_colli_target_->dest_ori() -
542  if_motor_->odometry_orientation() );
543  // need to consider minimum rotation velocity
544  if ( proposed_.rot > 0.f )
545  proposed_.rot = std::min( if_colli_target_->max_rotation(), std::max( cfg_min_rot_, proposed_.rot));
546  else
547  proposed_.rot = std::max(-if_colli_target_->max_rotation(), std::min(-cfg_min_rot_, proposed_.rot));
548 
549  occ_grid_->reset_old();
550 
551  } else {
552  // search for a path
553  search_->update( robo_grid_pos_.x, robo_grid_pos_.y,
554  (int)target_grid_pos_.x, (int)target_grid_pos_.y );
555  if ( search_->updated_successful() ) {
556  // path exists
557  local_grid_target_ = search_->get_local_target();
558  local_grid_trajec_ = search_->get_local_trajec();
559 
560  // coordinate transformation from grid coordinates to relative robot coordinates
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;
563 
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;
566 
567  // call appopriate drive mode
568  select_drive_mode_->set_local_target( local_target_.x, local_target_.y );
569  select_drive_mode_->set_local_trajec( local_trajec_.x, local_trajec_.y );
570  select_drive_mode_->update();
571  proposed_.x = select_drive_mode_->get_proposed_trans_x();
572  proposed_.y = select_drive_mode_->get_proposed_trans_y();
573  proposed_.rot = select_drive_mode_->get_proposed_rot();
574 
575  } else {
576  // stop
577  // logger->log_warn(name(), "Drive Mode: update not successful ---> stopping!");
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;
581  occ_grid_->reset_old();
582  }
583 
584  colli_data_.local_target = local_target_; // waypoints
585  colli_data_.local_trajec = local_trajec_; // collision-points
586  }
587  }
588 
589 
590  }
591 
592  if (cfg_write_spam_debug_) {
593  logger->log_debug(name(), "I want to realize %f , %f , %f", proposed_.x, proposed_.y, proposed_.rot);
594  }
595 
596  // check if occ-grid has been updated successfully
597  if( distance_to_next_target_ == 0.f ) {
598  logger->log_error(name(), "Cccupancy-grid update failed! Stop immediately");
599  proposed_.x = proposed_.y = proposed_.rot = 0.f;
600  motor_instruct_->stop();
601 
602  } else if( // check if emergency stop is needed
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_;
607 
608  float part_x = 0.f;
609  float part_y = 0.f;
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) ) );
613  }
614 
615  proposed_.x = part_x * max_v;
616  proposed_.y = part_y * max_v;
617 
618  logger->log_error(name(), "Emergency slow down: %f , %f , %f", proposed_.x, proposed_.y, proposed_.rot);
619 
620  emergency_motor_instruct_->drive( proposed_.x, proposed_.y, proposed_.rot );
621 
622  } else {
623  // Realize trans-rot proposal with realization module
624  motor_instruct_->drive( proposed_.x, proposed_.y, proposed_.rot );
625  }
626 }
627 
628 
629 /* **************************************************************************** */
630 /* Initialization */
631 /* **************************************************************************** */
632 /// Register all BB-Interfaces at the Blackboard.
633 void
634 ColliThread::open_interfaces()
635 {
636  if_motor_ = blackboard->open_for_reading<MotorInterface>(cfg_iface_motor_.c_str());
637  if_laser_ = blackboard->open_for_reading<Laser360Interface>(cfg_iface_laser_.c_str());
638  if_motor_->read();
639  if_laser_->read();
640 
641  if_colli_target_ = blackboard->open_for_writing<NavigatorInterface>(cfg_iface_colli_.c_str());
642  if_colli_target_->set_final( true );
643  if_colli_target_->write();
644 }
645 
646 
647 /// Initialize all modules used by the Colli
648 void
649 ColliThread::initialize_modules()
650 {
651  colli_data_.final = true;
652 
653  occ_grid_ = new LaserOccupancyGrid( if_laser_, logger, config, tf_listener);
654 
655  // set the cell width and heigth to 5 cm and the grid size to 7.5 m x 7.5 m.
656  // this are 750/5 x 750/5 grid cells -> (750x750)/5 = 22500 grid cells
657  occ_grid_->set_cell_width( occ_grid_cell_width_ );
658  occ_grid_->set_width( (int)((occ_grid_width_*100)/occ_grid_->get_cell_width()) );
659  occ_grid_->set_cell_height( occ_grid_cell_height_ );
660  occ_grid_->set_height( (int)((occ_grid_height_*100)/occ_grid_->get_cell_height()) );
661 
662  try {
663  // THIRD(!): the search component (it uses the occ grid (without the laser)
664  search_ = new Search( occ_grid_, logger, config );
665  } catch(Exception &e) {
666  logger->log_error(name(), "Could not created new search (%s)", e.what_no_backtrace());
667  delete occ_grid_;
668  throw;
669  }
670 
671  try {
672  // BEFORE DRIVE MODE: the motorinstruction set
673  if ( cfg_motor_instruct_mode_ == fawkes::colli_motor_instruct_mode_t::linear ) {
674  motor_instruct_ = (BaseMotorInstruct *)new LinearMotorInstruct( if_motor_,
675  frequency_,
676  logger,
677  config );
678  } else if ( cfg_motor_instruct_mode_ == fawkes::colli_motor_instruct_mode_t::quadratic ) {
679  motor_instruct_ = (BaseMotorInstruct *)new QuadraticMotorInstruct( if_motor_,
680  frequency_,
681  logger,
682  config );
683  } else {
684  logger->log_error(name(), "Motor instruct not implemented, use linear");
685  motor_instruct_ = (BaseMotorInstruct *)new LinearMotorInstruct( if_motor_,
686  frequency_,
687  logger,
688  config );
689  }
690  } catch(Exception &e) {
691  logger->log_error(name(), "Could not create MotorInstruct (%s", e.what_no_backtrace());
692  delete occ_grid_;
693  delete search_;
694  throw;
695  }
696 
697  try {
698  emergency_motor_instruct_ = (BaseMotorInstruct *)new EmergencyMotorInstruct( if_motor_,
699  frequency_,
700  logger,
701  config );
702  } catch(Exception &e) {
703  logger->log_error(name(), "Could not create EmergencyMotorInstruct (%s", e.what_no_backtrace());
704  delete occ_grid_;
705  delete search_;
706  delete motor_instruct_;
707  throw;
708  }
709 
710  try {
711  // AFTER MOTOR INSTRUCT: the motor propose values object
712  select_drive_mode_ = new SelectDriveMode( if_motor_, if_colli_target_, logger, config, cfg_escape_mode_ );
713  } catch(Exception &e) {
714  logger->log_error(name(), "Could not create SelectDriveMode (%s", e.what_no_backtrace());
715  delete occ_grid_;
716  delete search_;
717  delete motor_instruct_;
718  delete emergency_motor_instruct_;
719  throw;
720  }
721 
722  // Initialization of colli state machine:
723  // Currently nothing is to accomplish
724  colli_state_ = NothingToDo;
725 }
726 
727 
728 
729 
730 /* **************************************************************************** */
731 /* During Runtime */
732 /* **************************************************************************** */
733 /// read interface data from blackboard
734 void
735 ColliThread::interfaces_read()
736 {
737  if_laser_->read();
738  if_motor_->read();
739 }
740 
741 
742 /// Check if the interface data is valid, i.e. not outdated
743 bool
744 ColliThread::interface_data_valid()
745 {
746  Time now(clock);
747 
748  /* check if we have fresh data to fetch. An error has occured if
749  * a) laser or motor interface have no writer
750  * b) there is no new laser data for a while, or
751  * c) there is no motor data for a while and colli is currently moving
752  * d) transforms have not been updated in a while
753  */
754  if( !if_laser_->has_writer() || !if_motor_->has_writer() ) {
755  logger->log_warn(name(), "Laser or Motor dead, no writing instance for interfaces!!!");
756  return false;
757  } else if (if_laser_->timestamp()->is_zero()) {
758  logger->log_debug(name(), "No laser data");
759  return false;
760 
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_);
764  return false;
765 
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_);
769  return false;
770 
771  } else {
772  // check if transforms are up to date
773  tf::TimeCacheInterfacePtr cache = tf_listener->get_frame_cache(cfg_frame_laser_);
774  if( !cache ) {
775  logger->log_warn(name(), "No TimeCache for transform to laser_frame '%s'", cfg_frame_laser_.c_str());
776  return false;
777  }
778 
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());
782  return false;
783  }
784 
785  fawkes::Time laser_frame_latest(cache->get_latest_timestamp());
786  if (! laser_frame_latest.is_zero()) {
787  // not a static transform
788  float diff = (now - laser_frame_latest).in_sec();
789  if( diff > 2.f* cfg_iface_read_timeout_) {
790  logger->log_warn(name(), "Transform to laser frame '%s' is too old (%f > %f)",
791  cfg_frame_laser_.c_str(), diff, 2.f*cfg_iface_read_timeout_);
792  return false;
793  }
794  }
795 
796  // everything OK
797  return true;
798  }
799 }
800 
801 
802 
803 void
804 ColliThread::update_colli_state()
805 {
806  // initialize
807  if( target_new_ ) {
808  // new target!
809  colli_state_ = NothingToDo;
810  target_new_ = false;
811  }
812 
813  float cur_x = if_motor_->odometry_position_x();
814  float cur_y = if_motor_->odometry_position_y();
815  float cur_ori = normalize_mirror_rad( if_motor_->odometry_orientation() );
816 
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();
820 
821  bool orient = ( if_colli_target_->orientation_mode() == fawkes::NavigatorInterface::OrientationMode::OrientAtTarget
822  && std::isfinite(if_colli_target_->dest_ori()) );
823 
824  float target_dist = distance(target_x, target_y, cur_x, cur_y);
825 
826  bool is_driving = colli_state_ == DriveToTarget;
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_);
829 
830  /* Decide which status we need to switch to.
831  * We keep the current status, unless one of the following happens:
832  * 1) The target is far away
833  * -> we drive to the target via a pre-position
834  * 2) The target was initially far away, now exceeds a minimum-distance so that it can drive
835  * straight to the target without a pre-position.
836  * -> we drive to that target directly
837  * 3) The robot is considered to be "at target position" and exceeds a minimum angle to the target
838  * orientation
839  * -> we rotate towards the target rotation.
840  * 4) The new target is in a short distance (not as far as in case (2) yet)
841  * -> we drive to that target directly
842  *
843  * Special cases are also considered:
844  * 1') We reached the target position and are already adjusting orientation
845  * -> ONLY rotate at this point, and finish when rotation is over (avoid driving again)
846  * 2') We are driving straight to the target, but are not close enough yet to it to say "stop".
847  * -> continue drivint straight to the target, even if the distance would not trigger (2) from above
848  * anymore.
849  *
850  * 5) Other than that, we have nothing to do :)
851  */
852 
853  if( colli_state_ == OrientAtTarget ) { // case (1')
854  if ( !orient || ( fabs( normalize_mirror_rad(cur_ori - target_ori) ) < cfg_min_rot_dist_ ) )
855  colli_state_ = NothingToDo; // we don't need to rotate anymore; case
856  return;
857  }
858 
859  if( orient && ( target_dist >= cfg_min_long_dist_prepos_ ) ) { // case (1)
860  // We approach a point prior to the target, to adjust the orientation a little
861  float pre_pos_dist = cfg_target_pre_pos_;
862  if ( if_motor_->des_vx() < 0 )
863  pre_pos_dist = -pre_pos_dist;
864 
865  target_point_.x = target_x - ( pre_pos_dist * cos(target_ori) );
866  target_point_.y = target_y - ( pre_pos_dist * sin(target_ori) );
867 
868  colli_state_ = DriveToOrientPoint;
869  return;
870 
871  } else if( (target_dist >= cfg_min_long_dist_drive_) // case (2)
872  || (is_driving && target_dist >= cfg_min_drive_dist_) // case (2')
873  || (is_new_short_target && target_dist >= cfg_min_drive_dist_) ) { // case (4)
874  target_point_.x = target_x;
875  target_point_.y = target_y;
876  colli_state_ = DriveToTarget;
877  return;
878 
879  } else if ( orient && ( fabs( normalize_mirror_rad(cur_ori - target_ori) ) >= cfg_min_rot_dist_ ) ) { // case (3)
880  colli_state_ = OrientAtTarget;
881  return;
882 
883  } else { // case (5)
884  colli_state_ = NothingToDo;
885  return;
886  }
887 
888  return;
889 }
890 
891 
892 
893 /// Calculate all information out of the updated blackboard data
894 // robo_grid_pos_, laser_grid_pos_, target_grid_pos_ have to be updated!
895 // the targetPointX and targetPointY were calculated in the collis state machine!
896 void
897 ColliThread::update_modules()
898 {
899  float vx, vy, v;
900  vx = if_motor_->des_vx();
901  vy = if_motor_->des_vy();
902  v = std::sqrt( vx*vx + vy*vy );
903 
904  if ( !cfg_obstacle_inc_ ) {
905  // do not increase cell size
906  occ_grid_->set_cell_width( (int)occ_grid_cell_width_ );
907  occ_grid_->set_cell_height( (int)occ_grid_cell_height_ );
908 
909  } else {
910  // set the cell size according to the current speed
911  occ_grid_->set_cell_width( (int)std::max( (int)occ_grid_cell_width_,
912  (int)(5*fabs( v )+3) ) );
913  occ_grid_->set_cell_height((int)std::max( (int)occ_grid_cell_height_,
914  (int)(5*fabs( v )+3) ) );
915  }
916 
917  // Calculate discrete position of the laser
918  int laserpos_x = (int)(occ_grid_->get_width() / 2);
919  int laserpos_y = (int)(occ_grid_->get_height() / 2);
920 
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) );
924 
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() );
927 
928  // coordinate transformation for target point
929  float a_x = target_point_.x - if_motor_->odometry_position_x();
930  float a_y = target_point_.y - if_motor_->odometry_position_y();
931  float cur_ori = normalize_mirror_rad( if_motor_->odometry_orientation() );
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 ) );
934 
935  // calculation, where in the grid the target is, thats relative to the motorpos, so add it ;-)
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() );
938 
939  target_grid_x += robopos_x;
940  target_grid_y += robopos_y;
941 
942 
943  // check the target borders. if its out of the occ grid, put it back in by border checking
944  // with linear interpolation
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;
948  }
949 
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));
952  target_grid_x = 2;
953  }
954 
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));
957  target_grid_y = occ_grid_->get_height()-2;
958  }
959 
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));
962  target_grid_y = 2;
963  }
964 
965  // Robo increasement for robots
966  float robo_inc = 0.f;
967 
968  if ( if_colli_target_->security_distance() > 0.f )
969  robo_inc = if_colli_target_->security_distance();
970 
971  if ( cfg_obstacle_inc_ ) {
972  // calculate increasement due to speed
973  //float transinc = max(0.0,fabs( motor_instruct_->GetMotorCurrentTranslation()/2.0 )-0.35);
974  //float rotinc = max(0.0,fabs( motor_instruct_->GetMotorCurrentRotation()/3.5 )-0.4);
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 );
979 
980  // increase at least as much as "security distance"!
981  robo_inc = max( robo_inc, speedinc);
982 
983  // check against increasement limits
984  robo_inc = min( max_robo_inc_, robo_inc );
985  }
986 
987  // update the occgrid...
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 );
990 
991  // update the positions
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;
998 }
999 
1000 /// Check if we want to escape an obstacle
1001 bool
1002 ColliThread::check_escape()
1003 {
1004  static unsigned int cell_cost_occ = occ_grid_->get_cell_costs().occ;
1005  return ((float)occ_grid_->get_prob(robo_grid_pos_.x,robo_grid_pos_.y) == cell_cost_occ );
1006 }
Laser360Interface Fawkes BlackBoard Interface.
cart_coord_2d_t local_trajec
local trajectory
Definition: types.h:47
void wait()
Wait until minimum loop time has been reached.
Definition: wait.cpp:81
float x
Translation in x-direction.
Definition: types.h:61
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.
Definition: angle.h:62
int get_height()
Get the height of the grid.
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Definition: og_laser.cpp:431
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.
Definition: mutex.cpp:135
Storage for transforms and their parent.
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
Definition: types.h:46
STL namespace.
bool final
final-status
Definition: types.h:45
void update(int robo_x, int robo_y, int target_x, int target_y)
update complete plan things
A class for handling time.
Definition: time.h:91
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.
Definition: exception.cpp:661
Thread class encapsulation of pthreads.
Definition: thread.h:42
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:442
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
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.
Definition: og_laser.cpp:357
float dest_ori() const
Get dest_ori value.
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
float y
Translation in y-direction.
Definition: types.h:62
float vx() const
Get vx value.
float rot
Rotation around z-axis.
Definition: types.h:63
Polar coordinates.
Definition: types.h:85
OrientationMode orientation_mode() const
Get orientation_mode value.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
void reset_old()
Reset all old readings and forget about the world state!
Definition: og_laser.cpp:152
float odometry_orientation() const
Get odometry_orientation value.
float get_cache_time() const
Get cache time.
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 ...
Definition: og_laser.h:49
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
float vy() const
Get vy value.
bool is_final() const
Get final value.
Drive to the orientation point.
Definition: types.h:39
Base class for exceptions in Fawkes.
Definition: exception.h:36
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.
Definition: interface.cpp:477
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.
Definition: time.h:116
unsigned int occ
The cost for an occupied cell.
Definition: types.h:52
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
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.
int y
y coordinate
Definition: types.h:42
const char * name() const
Get name of thread.
Definition: thread.h:95
This is the plan class.
Definition: astar_search.h:47
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
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.
Definition: interface.cpp:718
float security_distance() const
Get security_distance value.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
float y
y coordinate
Definition: types.h:61
void set_local_trajec(float x, float y)
Set local trajectory point before update!
void mark_start()
Mark start of loop.
Definition: wait.cpp:70
void set_dest_dist(const float new_dest_dist)
Set dest_dist value.
float des_vx() const
Get des_vx value.
float r
distance
Definition: types.h:86
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.
Definition: types.h:38
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.
Definition: mutex.cpp:89
float dest_dist() const
Get dest_dist value.
MotorInterface Fawkes BlackBoard Interface.
bool is_escaping_enabled() const
Get escaping_enabled value.
float phi
angle
Definition: types.h:87
Mutex mutual exclusion lock.
Definition: mutex.h:32
float * distances() const
Get distances value.
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:70
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.
Definition: configurable.h:44
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)
Drive to the target.
Definition: types.h:40
Time wait utility.
Definition: wait.h:32
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.
Definition: types.h:37
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
int x
x coordinate
Definition: types.h:41
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.
float x
x coordinate
Definition: types.h:60