Hi Faruque,
I'm currently developing a class to facilitate my work with e-pucks in
Player, and the test is a target reaching with bug algorithm obstacle
avoidance. It isn't finished, but may be useful as an example code. In
specific, the bug algorithm don't stop the wall following if the wall
is circular for example, but work in rectangular obstacles because the
90 degrees angles discontinues the wall.
Renato Garcia
On Thu, 30 Apr 2009 10:32:24 +0100
M O Faruque Sarker <[email protected]> wrote:
> Hi all,
>
> I'm looking for sample e-puck code for target reaching with obstacle
> avoidance. I've found a plain target reaching code here:
> http://hades.mech.northwestern.edu/wiki/images/6/63/NUtest.c
>
> However, I'd like to add obstacle avoidance capability while reaching
> the target. I've also checked the runbreitenberg.c (in
> program/demo_swis_1). Has anyone integrated this with a target
> reaching code, such as found above?
>
> I'm also interested to learn about player/stage driver for e-puck. Can
> e-puck navigation be achieved using player/stage driver ?
>
> Thanks in advance,
> Faruque
>
> ¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬
> M Omar Faruque Sarker
> PhD Student,
> Robotics Intelligence Lab,
> University of Wales, Newport,
> Allt-yr-yn Campus, Newport, NP20 5XR, UK
> http://ril.newport.ac.uk/sarker/index.php
> Phone:+00441633 432548
> ¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬¬
#include <iostream>
#include "epuck.hpp"
#include <boost/assign/std/vector.hpp>
#include <algorithm>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <stdlib.h>
using namespace std;
using namespace boost::assign;
#define RIGHT_WALL 1
#define LEFT_WALL 2
void followWall(Epuck& epuck, int wallSide)
{
double wallAngleIdeal;
int irSensorLateral, irSensorDiagonal;
int angularSpeedFactor; // Factor to turn e-puck in correct direction.
if (wallSide == RIGHT_WALL)
{
wallAngleIdeal = -3.1415/2;
irSensorLateral = 2;
irSensorDiagonal = 1;
angularSpeedFactor = 1;
}
else
{
wallAngleIdeal = 3.1415/2;
irSensorLateral = 5;
irSensorDiagonal = 6;
angularSpeedFactor = -1;
}
Epuck::IrReadingArray irr(epuck.readIrSensors());
double wallAngle = std::min_element(irr.begin(), irr.end())->theta;
epuck.turn(wallAngle - wallAngleIdeal);
unsigned irRawLateral, irRawDiagonal;
double angularSpeed;
do
{
irRawLateral = epuck.readIrSensors().at(irSensorLateral).raw;
irRawDiagonal = epuck.readIrSensors().at(irSensorDiagonal).raw;
angularSpeed = angularSpeedFactor * ((float)irRawLateral - 3000)/8000;
if (irRawDiagonal > 1500) // E-puck turned to wall
{
angularSpeed += angularSpeedFactor * (irRawDiagonal - 1500.0)/1000.0;
}
epuck.setSpeed(0.03, angularSpeed);
}while(irRawLateral > 1000);
}
bool cc(Epuck& epuck)
{
return epuck.checkCollision();
}
int
main(int argc, char *argv[])
{
vector<Epuck::Device> devices;
devices += Epuck::MOTORS, Epuck::IR;
double startX = strtod(argv[1], NULL);
double startY = strtod(argv[2], NULL);
double startTheta = strtod(argv[3], NULL);
Epuck epuck(devices);
epuck.setPose(startX, startY, startTheta);
boost::function<bool ()> wallCheck(boost::bind(cc, boost::ref(epuck)));
while(true)
{
if(epuck.goTo(startX, startY+0.6, wallCheck))
{
// Success!!
break;
}
else
{
Epuck::IrReadingArray irr(epuck.readIrSensors());
unsigned irIndex = std::min_element(irr.begin(), irr.end())->index;
if (irIndex <= 3)
{
followWall(epuck, RIGHT_WALL);
}
else
{
followWall(epuck, LEFT_WALL);
}
}
}
return 0;
}
#include <iostream>
using namespace std;
#include "epuck.hpp"
#include <stdexcept>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
Epuck::Epuck(std::vector<Device> devices,
std::string playerHost, unsigned port)
:client(playerHost, port)
{
this->client.SetDataMode(PLAYER_DATAMODE_PULL);
this->client.SetReplaceRule(true);
BOOST_FOREACH(Device device, devices)
{
switch (device)
{
case Epuck::MOTORS:
this->position2d.reset(new PlayerCc::Position2dProxy(&this->client, 0));
this->position2d->RequestGeom();
break;
case Epuck::IR:
this->ir.reset(new PlayerCc::IrProxy(&this->client, 0));
this->ir->RequestGeom();
while (this->ir->GetCount() == 0)
{
this->client.Read();
}
break;
default:
throw std::runtime_error("Unknown epuck device");
}
}
}
Epuck::~Epuck()
{}
void
Epuck::updatePlayer()
{
this->client.Read();
clog << this->position2d->GetXPos() << '\t'
<< this->position2d->GetYPos() << '\t'
<< this->position2d->GetYaw() << endl;
}
void
Epuck::setPose(double x, double y, double theta)
{
this->position2d->SetOdometry (x, y, theta);
}
void
Epuck::setSpeed(double linear, double angular)
{
this->position2d->SetSpeed(linear, angular);
}
bool
Epuck::goTo(double targetX, double targetY,
boost::function<bool ()>& stopCheck)
{
double deltaX, deltaY, deltaL, deltaW;
double linearSpeed, angularSpeed;
do
{
this->updatePlayer();
if(stopCheck())
{
this->position2d->SetSpeed(0, 0);
return false;
}
deltaX = targetX - this->position2d->GetXPos();
deltaY = targetY - this->position2d->GetYPos();
deltaL = sqrt(pow(deltaX, 2) + pow(deltaY, 2));
deltaW = PlayerCc::normalize(atan2(deltaY, deltaX) -
this->position2d->GetYaw());
linearSpeed = deltaL > 0.07 ? 0.07 : deltaL;
linearSpeed /= pow(1+fabs(deltaW),2);
angularSpeed = deltaW * 0.3;
this->position2d->SetSpeed(linearSpeed, angularSpeed);
}while (deltaL > Epuck::LINEAR_TOLERANCE);
this->position2d->SetSpeed(0, 0);
return true;
}
bool
Epuck::go(double deltaX, double deltaY,
boost::function<bool ()>& stopCheck)
{
this->updatePlayer();
double deltaXWorld = deltaX * cos(this->position2d->GetYaw()) -
deltaY * sin(this->position2d->GetYaw());
double deltaYWorld = deltaX * sin(this->position2d->GetYaw()) +
deltaY * cos(this->position2d->GetYaw());
return this->goTo(this->position2d->GetXPos() + deltaXWorld,
this->position2d->GetYPos() + deltaYWorld,
stopCheck);
}
void
Epuck::turnTo(double targetTheta)
{
this->updatePlayer();
double deltaT;
do
{
this->updatePlayer();
deltaT = PlayerCc::normalize(targetTheta - this->position2d->GetYaw());
this->position2d->SetSpeed(0, deltaT);
}while (fabs(deltaT) > Epuck::ANGULAR_TOLERANCE);
this->position2d->SetSpeed(0, 0);
}
bool
Epuck::checkCollision()
{
this->updatePlayer();
for(int i = 0; i < Epuck::N_IR_SENSORS; ++i)
{
if (this->ir->GetVoltage(i) > Epuck::IR_COLLISION_THRESHOLD)
{
return true;
}
}
return false;
}
bool
Epuck::checkCollision(std::vector<double>& collisions)
{
collisions.clear();
this->updatePlayer();
for(int i = 0; i < Epuck::N_IR_SENSORS; ++i)
{
if (this->ir->GetVoltage(i) > Epuck::IR_COLLISION_THRESHOLD)
{
collisions.push_back(this->ir->GetPose(i).pyaw);
}
}
return (!collisions.empty());
}
Epuck::IrReadingArray
Epuck::readIrSensors()
{
this->updatePlayer();
IrReadingArray irReadings;
for(int i = 0; i < Epuck::N_IR_SENSORS; ++i)
{
irReadings.at(i).index = (unsigned)i;
irReadings.at(i).raw = (unsigned)this->ir->GetVoltage(i);
irReadings.at(i).range = this->ir->GetRange(i);
irReadings.at(i).theta = this->ir->GetPose(i).pyaw;
}
return irReadings;
}
#ifndef EPUCK_HPP
#define EPUCK_HPP
#include <cmath>
#include <string>
#include <vector>
#include <boost/array.hpp>
#include <boost/function.hpp>
#include <boost/scoped_ptr.hpp>
#include <libplayerc++/playerc++.h>
class Epuck
{
class IrReading
{
public:
unsigned raw; // Raw IR sensor reading
double range; // Raw IR sensor reading converted to distance in meters
unsigned index; // Index of IR sensor in e-puck
double theta; // Angle of IR sensor in e-puck
bool operator<(const IrReading& irReading)const
{
// A greater raw reading is a lesser object distance
return (this->raw > irReading.raw);
}
};
private:
static const float LINEAR_TOLERANCE = 0.03;
static const float ANGULAR_TOLERANCE = 0.01;
static const unsigned IR_COLLISION_THRESHOLD = 2000;
static const unsigned N_IR_SENSORS = 8;
public:
enum Device
{
MOTORS,
IR
};
typedef boost::array<IrReading, Epuck::N_IR_SENSORS> IrReadingArray;
Epuck(std::vector<Device> devices,
std::string playerHost="localhost", unsigned port=6665);
virtual ~Epuck();
void setPose(double x, double y, double theta);
void setSpeed(double linear, double angular);
void setVel(double x, double y);
/** Go to a point in world coordinates.
* @param targetX Coordinate x.
* @param targetY Coordinate y.
*
* @return True if the target was reached, false else.
*/
bool goTo(double targetX, double targetY,
boost::function<bool ()>& stopCheck);
/** Displace a delta in e-puck coordinates.
* @param deltaX Displacement in x coordinate (ahead).
* @param deltaY Displacement in y coordinate (lateral).
* @return True if displacement was done, false else.
*/
bool go(double deltaX, double deltaY,
boost::function<bool ()>& stopCheck);
/** Turn to a angle in world coordinates
* @param targetTheta Target angle
*/
void turnTo(double targetTheta);
/** Turn a delta angle
* @param deltaTheta Angle to turn
*/
void turn(double deltaTheta)
{
this->turnTo(this->position2d->GetYaw() + deltaTheta);
}
bool checkCollision();
bool checkCollision(std::vector<double>& collisions);
IrReadingArray readIrSensors();
private:
PlayerCc::PlayerClient client;
boost::scoped_ptr<PlayerCc::Position2dProxy> position2d;
boost::scoped_ptr<PlayerCc::IrProxy> ir;
void updatePlayer();
};
#endif // EPUCK_HPP
_______________________________________________
E-puck-user mailing list
[email protected]
https://mail.gna.org/listinfo/e-puck-user
