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