Better wall avoidance.

This commit is contained in:
2016-06-21 13:33:57 -04:00
parent df09ae9dfe
commit 1283ccfff1
6 changed files with 68 additions and 31 deletions

View File

@@ -35,8 +35,8 @@ window
floorplan floorplan
( (
bitmap "maps/hospital_section.png" bitmap "maps/cave.png"
size [ 64 26 1.500 ] size [ 16 16 1.500 ]
) )
ant_bot ant_bot
@@ -65,4 +65,4 @@ ant_bot
name "ant_robot_4" name "ant_robot_4"
pose [ 6.432 5.895 0 -135.000 ] pose [ 6.432 5.895 0 -135.000 ]
color "green" color "green"
) )

View File

@@ -25,16 +25,14 @@ define ant_sensor sensor
( (
size [0.01 0.05 0.01 ] size [0.01 0.05 0.01 ]
range [0 5.0] range [0 5.0]
fov 60 fov 222
samples 1 samples 222
color_rgba [ 0 1 0 0.2 ] color_rgba [ 0 1 0 0.2 ]
) )
define ant_sonars ranger define ant_sonars ranger
( (
ant_sensor( pose [ 0 0 0 -60 ] ) ant_sensor( pose [ 0 0 0 0 ] )
ant_sensor( pose [ 0 0 0 0 ] )
ant_sensor( pose [ 0 0 0 60 ] )
) )
define ant_bot position define ant_bot position

View File

@@ -23,28 +23,63 @@
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
*************************************************************************************/ *************************************************************************************/
#include <limits>
#include <sys/time.h>
#include "ias_robot.hpp" #include "ias_robot.hpp"
static const long HALF_SECOND_USEC = 500000;
static const double MIN_DIST_M = 1.5;
IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port) : Robot(hostname, port) { IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port) : Robot(hostname, port) {
std::cout << "Creating IAS-SS robot on host \"" << hostname << "\" and port " << port << "." << std::endl; std::cout << "Creating IAS-SS robot on host \"" << hostname << "\" and port " << port << "." << std::endl;
} }
IASSS_Robot::~IASSS_Robot() { IASSS_Robot::~IASSS_Robot() {
std::cout << "Destroying IAS-SS robot." << std::endl; std::cout << "Destroying IAS-SS robot on " << _host_name << ":" << _port << std::endl;
} }
void IASSS_Robot::run() { void IASSS_Robot::run() {
int rv;
long then, now, delta, wait;
struct timeval tv;
double dist = std::numeric_limits<double>::infinity();
double dist_l = 0.0;
double dist_r = 0.0;
_p_client->Read(); _p_client->Read();
rv = gettimeofday(&tv, NULL);
if(_r_proxy->GetRange(1) < 1.0) { then = tv.tv_usec;
if(_r_proxy->GetRange(0) > 1.0) /******************************************************************************
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-45)); * WALL AVOIDANCE START *
else if(_r_proxy->GetRange(2) > 1.0) ******************************************************************************/
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(45)); // Check if there is something in front of the robot.
for(int i = 96; i < 126; i++)
dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist;
if(dist < MIN_DIST_M) {
for(unsigned int i = 0; i < 96; i++)
dist_r += _r_proxy->GetRange(i);
dist_r /= 96;
for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++)
dist_l += _r_proxy->GetRange(i);
dist_l /= (_r_proxy->GetRangeCount() - 126);
if(dist_r >= dist_l)
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-20));
else else
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(180)); _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(20));
} else { } else
_p_proxy->SetSpeed(0.3f, 0.0f); _p_proxy->SetSpeed(0.4f, 0.0f);
} /******************************************************************************
sleep(1); * WALL AVOIDANCE END *
******************************************************************************/
rv = gettimeofday(&tv, NULL);
now = tv.tv_usec;
delta = now - then;
// Sleep for a bit before finishing this control iteration.
wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC;
usleep(wait);
} }

View File

@@ -35,21 +35,19 @@ const uint32_t NUM_ROBOTS = 4;
static bool done = false; static bool done = false;
extern "C" { extern "C" void handler(int signal) {
void handler(int signal) { done = true;
done = true; }
}
void * robot_thread(void * arg) { extern "C" void * robot_thread(void * arg) {
IASSS_Robot * robot = static_cast<IASSS_Robot *>(arg); IASSS_Robot * robot = static_cast<IASSS_Robot *>(arg);
std::cout << "Running robot thread." << std::endl; std::cout << "Running robot thread." << std::endl;
while(!done) while(!done)
robot->run(); robot->run();
return NULL; return NULL;
}
} }
int main(int argc, char **argv) { int main(int argc, char **argv) {

View File

@@ -26,9 +26,13 @@
#include "robot.hpp" #include "robot.hpp"
Robot::Robot(std::string hostname, uint32_t port) { Robot::Robot(std::string hostname, uint32_t port) {
_host_name = hostname;
_port = port;
_p_client = new PlayerCc::PlayerClient(hostname, port); _p_client = new PlayerCc::PlayerClient(hostname, port);
_p_proxy = new PlayerCc::Position2dProxy(_p_client, 0); _p_proxy = new PlayerCc::Position2dProxy(_p_client, 0);
_r_proxy = new PlayerCc::RangerProxy(_p_client, 0); _r_proxy = new PlayerCc::RangerProxy(_p_client, 0);
_p_proxy->RequestGeom(); _p_proxy->RequestGeom();
_r_proxy->RequestGeom(); _r_proxy->RequestGeom();
_r_proxy->RequestConfigure(); _r_proxy->RequestConfigure();

View File

@@ -41,6 +41,8 @@ public:
virtual void run() = 0; virtual void run() = 0;
protected: protected:
int _port;
std::string _host_name;
PlayerCc::PlayerClient * _p_client; PlayerCc::PlayerClient * _p_client;
PlayerCc::Position2dProxy * _p_proxy; PlayerCc::Position2dProxy * _p_proxy;
PlayerCc::RangerProxy * _r_proxy; PlayerCc::RangerProxy * _r_proxy;