From cdbfdcdb5a56ef8707e2fe396fd0e5cdc1731d17 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 5 Apr 2013 01:49:25 +0200 Subject: digital/io-hub/src/apbirthday: add obstacles to path finding class --- digital/io-hub/src/common-cc/move.cc | 2 +- digital/io-hub/src/common-cc/obstacles.cc | 14 ++++++++++++++ digital/io-hub/src/common-cc/obstacles.hh | 3 +++ 3 files changed, 18 insertions(+), 1 deletion(-) (limited to 'digital/io-hub') 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. -- cgit v1.2.3