summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-05 01:49:25 +0200
committerNicolas Schodet2013-04-05 01:49:25 +0200
commitcdbfdcdb5a56ef8707e2fe396fd0e5cdc1731d17 (patch)
tree78c8379e1d9837b6f936c14a5839c4ac583f3716 /digital
parente12d94361fa1e9cef8ef5dfdde492b747d79b74e (diff)
digital/io-hub/src/apbirthday: add obstacles to path finding class
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/common-cc/move.cc2
-rw-r--r--digital/io-hub/src/common-cc/obstacles.cc14
-rw-r--r--digital/io-hub/src/common-cc/obstacles.hh3
3 files changed, 18 insertions, 1 deletions
diff --git a/digital/io-hub/src/common-cc/move.cc b/digital/io-hub/src/common-cc/move.cc
index 27de0c34..a4d490bc 100644
--- a/digital/io-hub/src/common-cc/move.cc
+++ b/digital/io-hub/src/common-cc/move.cc
@@ -209,7 +209,7 @@ Move::path_init ()
robot->asserv.get_position (current_pos);
// Give the current position of the bot to the path module
robot->path.reset ();
- //TODO add obstacles.
+ robot->obstacles.add_obstacles (robot->path);
robot->path.endpoints (current_pos.v, final_.v);
// Update the path module
slow_ = 0;
diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc
index 1c02cce6..882744e8 100644
--- a/digital/io-hub/src/common-cc/obstacles.cc
+++ b/digital/io-hub/src/common-cc/obstacles.cc
@@ -143,3 +143,17 @@ Obstacles::blocking (const vect_t &robot, const vect_t &dest) const
return false;
}
+void
+Obstacles::add_obstacles (Path &path) const
+{
+ int index = 0;
+ for (int i = 0; i < obstacles_nb_; i++)
+ {
+ if (obstacles_[i].valid)
+ {
+ path.obstacle (index++, obstacles_[i].pos,
+ obstacle_radius_mm + clearance_mm + BOT_SIZE_SIDE);
+ }
+ }
+}
+
diff --git a/digital/io-hub/src/common-cc/obstacles.hh b/digital/io-hub/src/common-cc/obstacles.hh
index 486c64b2..6b65dfc6 100644
--- a/digital/io-hub/src/common-cc/obstacles.hh
+++ b/digital/io-hub/src/common-cc/obstacles.hh
@@ -24,6 +24,7 @@
//
// }}}
#include "defs.hh"
+#include "path.hh"
/// Handle obstacles "database".
///
@@ -44,6 +45,8 @@ class Obstacles
/// 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;
+ /// Setup obstacles in path finding object.
+ void add_obstacles (Path &path) const;
public:
/// Estimated obstacle radius. The obstacle may be larger than at the
/// detected edge.