From b3392b2c9b5e808e868b092422d7b143a82c4628 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 31 Mar 2013 19:28:24 +0200 Subject: digital/io-hub/src/apbirthday: add radar system and obstacles handling --- digital/io-hub/src/common-cc/obstacles.cc | 105 ++++++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 digital/io-hub/src/common-cc/obstacles.cc (limited to 'digital/io-hub/src/common-cc/obstacles.cc') diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc new file mode 100644 index 00000000..d98a92ba --- /dev/null +++ b/digital/io-hub/src/common-cc/obstacles.cc @@ -0,0 +1,105 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "obstacles.hh" + +extern "C" { +#include "modules/math/geometry/distance.h" +} + +#ifdef TARGET_host +# include "robot.hh" +#endif + +Obstacles::Obstacles () + : changed_ (false) +{ + for (int i = 0; i < obstacles_nb_; i++) + obstacles_[i].valid = 0; +} + +void +Obstacles::update () +{ + for (int i = 0; i < obstacles_nb_; i++) + { + if (obstacles_[i].valid) + obstacles_[i].valid--; + } +#ifdef TARGET_host + if (changed_) + { + SimuReport &r = robot->hardware.simu_report; + vect_t o[obstacles_nb_]; + int o_nb = 0; + for (int i = 0; i < obstacles_nb_; i++) + { + if (obstacles_[i].valid) + o[o_nb++] = obstacles_[i].pos; + } + r.pos (o, o_nb, 0); + } +#endif + changed_ = false; +} + +void +Obstacles::add (const vect_t &pos) +{ + // Try to merge, and find the oldest on the way. + int oldest_i = 0, oldest_i_valid = valid_new_; + for (int i = 0; i < obstacles_nb_; i++) + { + if (obstacles_[i].valid) + { + if (distance_point_point (&pos, &obstacles_[i].pos) < same_mm_) + { + // Replace if older. + if (obstacles_[i].valid < valid_new_) + { + obstacles_[i].pos = pos; + obstacles_[i].valid = valid_new_; + changed_ = true; + } + return; + } + } + if (obstacles_[i].valid < oldest_i_valid) + { + oldest_i = i; + oldest_i_valid = obstacles_[i].valid; + } + } + // If not found, replace the oldest. + obstacles_[oldest_i].pos = pos; + obstacles_[oldest_i].valid = valid_new_; + changed_ = true; +} + +bool +Obstacles::blocking (const vect_t &robot, const vect_t &dest) const +{ + // TODO + return false; +} + -- cgit v1.2.3