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/defs.hh | 1 + digital/io-hub/src/common-cc/obstacles.cc | 105 +++++++++++++++++++++++ digital/io-hub/src/common-cc/obstacles.hh | 68 +++++++++++++++ digital/io-hub/src/common-cc/radar.cc | 101 ++++++++++++++++++++++ digital/io-hub/src/common-cc/radar.hh | 74 ++++++++++++++++ digital/io-hub/src/common-cc/simu_report.host.cc | 42 +++++++++ digital/io-hub/src/common-cc/simu_report.host.hh | 42 +++++++++ 7 files changed, 433 insertions(+) create mode 100644 digital/io-hub/src/common-cc/obstacles.cc create mode 100644 digital/io-hub/src/common-cc/obstacles.hh create mode 100644 digital/io-hub/src/common-cc/radar.cc create mode 100644 digital/io-hub/src/common-cc/radar.hh create mode 100644 digital/io-hub/src/common-cc/simu_report.host.cc create mode 100644 digital/io-hub/src/common-cc/simu_report.host.hh (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/defs.hh b/digital/io-hub/src/common-cc/defs.hh index 4475b299..72053720 100644 --- a/digital/io-hub/src/common-cc/defs.hh +++ b/digital/io-hub/src/common-cc/defs.hh @@ -23,6 +23,7 @@ // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} +#include "ucoolib/common.hh" /// General purpose definitions. 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; +} + diff --git a/digital/io-hub/src/common-cc/obstacles.hh b/digital/io-hub/src/common-cc/obstacles.hh new file mode 100644 index 00000000..c3d2bc34 --- /dev/null +++ b/digital/io-hub/src/common-cc/obstacles.hh @@ -0,0 +1,68 @@ +#ifndef obstacles_hh +#define obstacles_hh +// 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 "defs.hh" + +/// Handle obstacles "database". +/// +/// At each cycle, obstacle detections can come and go. This class will keep +/// tracks of previously seen obstacles, and merge new detection events with +/// past history. +/// +/// This class also decide if an obstacle is in the robot way. +class Obstacles +{ + public: + /// Constructor. + Obstacles (); + /// To be called at each cycle. + void update (); + /// Add an obstacle detection. + void add (const vect_t &pos); + /// Return true if there is an obstacle near the robot while going to a + /// destination point. + bool blocking (const vect_t &robot, const vect_t &dest) const; + private: + /// Validity period of a new obstacle. + static const int valid_new_ = 125; + /// Distance under which obstacles are considered the same. + static const int same_mm_ = 150; + /// Set when changed since last update. + bool changed_; + /// Single obstacle. + struct Obstacle + { + /// Position. + vect_t pos; + /// Validity period as a number of cycles, or 0 if unvalid. + int valid; + }; + /// Number of obstacles. + static const int obstacles_nb_ = 4; + /// Obstacles array. + Obstacle obstacles_[obstacles_nb_]; +}; + +#endif // obstacles_hh diff --git a/digital/io-hub/src/common-cc/radar.cc b/digital/io-hub/src/common-cc/radar.cc new file mode 100644 index 00000000..d7f2a96f --- /dev/null +++ b/digital/io-hub/src/common-cc/radar.cc @@ -0,0 +1,101 @@ +// 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 "radar.hh" + +/// Maximum distance for a sensor reading to be ignored if another sensor is +// nearer. +static const int far_mm = 250; + +void +Radar::update (const Position &robot_pos, Obstacles &obstacles) +{ + int i, j; + vect_t ray; + // Compute hit points for each sensor and eliminate invalid ones. + vect_t hit[sensors_nb_]; + int dist_mm[sensors_nb_]; + for (i = 0; i < sensors_nb_; i++) + { + dist_mm[i] = sensors_[i].sensor->get (); + if (dist_mm[i] != -1) + { + hit[i] = sensors_[i].pos; + vect_rotate_uf016 (&hit[i], robot_pos.a); + vect_translate (&hit[i], &robot_pos.v); + vect_from_polar_uf016 (&ray, dist_mm[i], + robot_pos.a + sensors_[i].a); + vect_translate (&hit[i], &ray); + if (valid (i, hit[i])) + { + vect_from_polar_uf016 (&ray, obstacle_edge_radius_mm_, + robot_pos.a + sensors_[i].a); + vect_translate (&hit[i], &ray); + } + else + dist_mm[i] = -1; + } + } + // Ignore sensor results too far from other sensors of the same group. + for (i = 0; i < sensors_nb_ - 1; i++) + { + if (dist_mm[i] != -1) + { + for (j = i + 1; j < sensors_nb_ && !sensors_[j].new_group; j++) + { + if (dist_mm[j] != -1) + { + if (dist_mm[i] + far_mm < dist_mm[j]) + dist_mm[j] = -1; + else if (dist_mm[j] + far_mm < dist_mm[i]) + dist_mm[i] = -1; + } + } + } + } + // Compute hit point from all sensors in the same group. + vect_t hit_center = { 0, 0 }; + int hit_nb = 0; + for (i = 0; i < sensors_nb_; i++) + { + if (dist_mm[i] != -1) + { + vect_add (&hit_center, &hit[i]); + hit_nb++; + } + // If last of group or last sensor, may add obstacle. + if ((i == sensors_nb_ - 1 || sensors_[i + 1].new_group) && hit_nb) + { + if (hit_nb > 1) + { + hit_center.x /= hit_nb; + hit_center.y /= hit_nb; + } + obstacles.add (hit_center); + hit_center.x = 0; + hit_center.y = 0; + hit_nb = 0; + } + } +} + diff --git a/digital/io-hub/src/common-cc/radar.hh b/digital/io-hub/src/common-cc/radar.hh new file mode 100644 index 00000000..d2cb2d83 --- /dev/null +++ b/digital/io-hub/src/common-cc/radar.hh @@ -0,0 +1,74 @@ +#ifndef radar_hh +#define radar_hh +// 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 "defs.hh" +#include "ucoolib/dev/usdist/usdist.hh" +#include "obstacles.hh" + +/// Describe a radar sensor. +struct RadarSensor +{ + /// US distance sensor. + ucoo::UsDist *sensor; + /// Position relative to the robot center. + vect_t pos; + /// Angle relative to the robot X axis. + uint16_t a; + /// Part of an new sensor group? + bool new_group; +}; + +/// Handle any distance sensors information to extract useful data. This +/// includes: +/// - combining several sensors information for a more precise obstacle +/// position, +/// - ignoring obstacles not in the playground. +class Radar +{ + public: + /// Margin to be considered inside the playground. An obstacle can not be + /// exactly at the playground edge. + static const int margin_mm = 150; + protected: + /// Constructor. + Radar (int obstacle_edge_radius_mm, RadarSensor *sensors, int sensors_nb) + : obstacle_edge_radius_mm_ (obstacle_edge_radius_mm), + sensors_ (sensors), sensors_nb_ (sensors_nb) { } + /// Define exclusion area, return true if point is valid. + virtual bool valid (int sensor_index, vect_t &p) = 0; + public: + /// Update radar view. Record found obstacles. + void update (const Position &robot_pos, Obstacles &obstacles); + private: + /// Estimated obstacle edge radius. As the sensors detect obstacle edge, + /// this is added to position obstacle center. + const int obstacle_edge_radius_mm_; + /// Sensors description. + RadarSensor *sensors_; + /// Number of sensors. + int sensors_nb_; +}; + +#endif // radar_hh diff --git a/digital/io-hub/src/common-cc/simu_report.host.cc b/digital/io-hub/src/common-cc/simu_report.host.cc new file mode 100644 index 00000000..2a3f98fd --- /dev/null +++ b/digital/io-hub/src/common-cc/simu_report.host.cc @@ -0,0 +1,42 @@ +// 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 "simu_report.host.hh" + +SimuReport::SimuReport (ucoo::Host &host) + : node_ (host.get_node ()) +{ + std::string instance (host.get_instance ()); + pos_mtype_ = node_.reserve (instance + ":pos-report"); +} + +void +SimuReport::pos (vect_t *pos, int pos_nb, uint8_t id) +{ + ucoo::mex::Msg msg (pos_mtype_); + msg.push ("B") << id; + for (; pos_nb; pos++, pos_nb--) + msg.push ("hh") << pos->x << pos->y; + node_.send (msg); +} + diff --git a/digital/io-hub/src/common-cc/simu_report.host.hh b/digital/io-hub/src/common-cc/simu_report.host.hh new file mode 100644 index 00000000..8e8929ad --- /dev/null +++ b/digital/io-hub/src/common-cc/simu_report.host.hh @@ -0,0 +1,42 @@ +#ifndef simu_report_host_hh +#define simu_report_host_hh +// 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 "ucoolib/arch/host/host.hh" +#include "defs.hh" + +/// Report internal information to be displayed by simulator interface. +class SimuReport +{ + public: + /// Constructor. + SimuReport (ucoo::Host &host); + /// Report positions. + void pos (vect_t *pos, int pos_nb, uint8_t id); + private: + ucoo::mex::Node &node_; + ucoo::mex::mtype_t pos_mtype_; +}; + +#endif // simu_report_host_hh -- cgit v1.2.3