Better wall avoidance.
This commit is contained in:
@@ -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"
|
||||||
)
|
)
|
||||||
|
@@ -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
|
||||||
|
@@ -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);
|
||||||
}
|
}
|
||||||
|
20
ias_ss.cpp
20
ias_ss.cpp
@@ -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) {
|
||||||
|
@@ -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();
|
||||||
|
@@ -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;
|
||||||
|
Reference in New Issue
Block a user