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
47using namespace fawkes;
48using namespace std;
49
50/** @class ColliThread "colli_thread.h"
51 * Thread that performs the navigation and collision avoidance algorithms.
52 */
53
54/** Constructor. */
55ColliThread::ColliThread() : Thread("ColliThread", Thread::OPMODE_CONTINUOUS), vis_thread_(0)
56{
57 mutex_ = new Mutex();
58}
59
60/** Destructor. */
62{
63 delete mutex_;
64}
65
66void
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;
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();
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()) {
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
187void
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 */
212void
213ColliThread::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 */
221bool
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 */
233void
234ColliThread::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 */
248void
249ColliThread::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. */
269void
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
283void
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 {
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();
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()) {
306 "Unable to transform %s to %s.\n%s",
307 cfg_frame_base_.c_str(),
308 cfg_frame_laser_.c_str(),
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/* **************************************************************************** */
394void
395ColliThread::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//
452void
453ColliThread::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_) {
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
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.
645void
646ColliThread::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
659void
660ColliThread::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
731void
732ColliThread::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
739bool
740ColliThread::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_) {
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_) {
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) {
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)) {
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_) {
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
809void
810ColliThread::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!
903void
904ColliThread::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
1011bool
1012ColliThread::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 noexcept
Get primary string.
Definition: exception.cpp:639
virtual const char * what_no_backtrace() const noexcept
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:714
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
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