Example Source Code:
Player Project Driver plus Example Player Client Program
For the WheelCommander WC-132
Description
This example includes a driver for Player 2.x and 3.x, as well as a simple wander example.
We have provided a CMake build file, which is how Player 3.x, drivers, and client programs are built. With it you can build the driver and example program under Ubuntu Linux, Mandriva Linux, Mac OS X Leopard or Snow Leopard (requires many MacPorts packages), and most versions of Microsoft Windows. You will also need our binary libaries for your platform; these will be released soon.
Download
wc132_driver.cpp - CPP source
wc132.cfg - Player configuration
wander.cpp - CPP source
Player-Stage.zip - Complete Driver including Example Code
Source Code
wc132_driver.cpp
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2003
* Brian Gerkey, Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*
* Desc: Loadable Player driver for the Nubotics WheelCommander WC-132
* Author: Pete Skeggs
* Date: 27 Jan 2009
* Updated: 6 Dec 2010 for Player 3.x
* derived from multidriver.cc example
*/
// ONLY if you need something that was #define'd as a result of configure
// (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so:
/*
#if HAVE_CONFIG_H
#include <config.h>
#endif
*/
#if !defined (WIN32)
#include <unistd.h>
#endif
#include <string.h>
#include <libplayercore/playercore.h>
#include <math.h>
#include <netinet/in.h>
#include <NdiCmd.h>
#include <Logger.h>
#define TIMEOUT_MS 500 // number of ms to wait for wc132 to respond
#define ANG_CONVERSION_FACTOR ((2 * 3.1415926 / 360.0)/10)
#define VEL_CONVERSION_FACTOR ((25.4/1000.0)/10)
#define ROT_CONVERSION_FACTOR ANG_CONVERSION_FACTOR
// Normalize angle to domain -pi, pi
#ifndef NORMALIZE
#define NORMALIZE(z) atan2(sin(z), cos(z))
#endif
#define PLAYER3 1 // undefine for player 2.x
const char* copyright_notice =
"\n * Player Project Driver for Nubotics WheelCommander WC-132 [http://www.nubotics.com]\n"
" * Copyright 2006-2011 Noetic Design, Inc.\n"
" * Released under the GNU General Public License v2.\n";
////////////////////////////////////////////////////////////////////////////////
// The class for the driver
#ifdef PLAYER3
#pragma message "Player 3.x detected; using ThreadedDriver class"
class WC132Driver : public ThreadedDriver
#else
#pragma message "Player 2.x detected; using Driver class"
class WC132Driver : public Driver
#endif
{
public:
// Constructor; need that
WC132Driver(ConfigFile* cf, int section);
// Must implement the following methods.
#ifdef PLAYER3
virtual int MainSetup();
virtual void MainQuit();
#else
virtual int Setup();
virtual int Shutdown();
#endif
virtual int ProcessMessage(QueuePointer & resp_queue,
player_msghdr * hdr,
void * data);
private:
// Main function for device thread.
virtual void Main();
void ProcessPos2dGeomReq(player_msghdr_t* hdr);
void ProcessAioData(player_msghdr_t* hdr, player_aio_data_t &data);
void ProcessDioCommand(player_msghdr_t* hdr, player_dio_cmd_t &data); // outputs
void ProcessDioData(player_msghdr_t* hdr, player_dio_data_t &data); // inputs
void ProcessPos2dVelCmd(player_msghdr_t* hdr, player_position2d_cmd_vel_t &data);
void ProcessPos2dPosCmd(player_msghdr_t* hdr, player_position2d_cmd_pos_t &data);
void UpdateData(void);
// My position interface
player_devaddr_t m_position_addr;
player_position2d_data_t m_pos_data;
int m_timestamp_ms;
// My dio interface
player_devaddr_t m_dio_addr;
// My aio interface
player_devaddr_t m_aio_addr;
// My power interface
player_devaddr_t m_power_addr;
player_power_data_t m_power_data;
// WC-132 specific members
float voltages[8];
int m_baud_rate;
NDICMD_DECLSPEC NdiCmd *pNdiCmd;
WheelCommander *pWC;
// robot-specific members
double m_width;
double m_length;
double m_height;
// operational parameters
BOOL m_quiet;
};
// A factory creation function, declared outside of the class so that it
// can be invoked without any object context (alternatively, you can
// declare it static in the class). In this function, we create and return
// (as a generic Driver*) a pointer to a new instance of this driver.
Driver* WC132Driver_Init(ConfigFile* cf, int section)
{
// Create and return a new instance of this driver
return((Driver*) (new WC132Driver(cf, section)));
}
// A driver registration function, again declared outside of the class so
// that it can be invoked without object context. In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void WC132Driver_Register(DriverTable* table)
{
printf("\n ** wc132_driver plugin v1.0 **");
if (!player_quiet_startup)
puts(copyright_notice);
table->AddDriver("wc132", WC132Driver_Init);
}
////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.
/* need the extern to avoid C++ name-mangling */
extern "C"
{
int player_driver_init(DriverTable* table)
{
puts(" wc132 driver plugin init\n");
WC132Driver_Register(table);
return(0);
}
}
////////////////////////////////////////////////////////////////////////////////
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
WC132Driver::WC132Driver(ConfigFile* cf, int section)
#ifdef PLAYER3
: ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
#else
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
#endif
{
pNdiCmd = NULL;
pWC = NULL;
m_baud_rate = -1;
m_timestamp_ms = 0;
memset(&m_pos_data, 0, sizeof(player_position2d_data_t));
// Create my position interface
if (cf->ReadDeviceAddr(&m_position_addr, section,
"provides", PLAYER_POSITION2D_CODE, -1, NULL))
{
PLAYER_ERROR("Could not read position2d ID ");
SetError(-1);
return;
}
if (AddInterface(m_position_addr))
{
PLAYER_ERROR("Could not add position2d interface ");
SetError(-1);
return;
}
// Create dio interface
if (cf->ReadDeviceAddr(&m_dio_addr, section,
"provides", PLAYER_DIO_CODE, -1, NULL))
{
PLAYER_ERROR("Could not read dio ID ");
SetError(-1);
return;
}
if (AddInterface(m_dio_addr))
{
PLAYER_ERROR("Could not add dio interface ");
SetError(-1);
return;
}
// Create aio interface
if (cf->ReadDeviceAddr(&m_aio_addr, section,
"provides", PLAYER_AIO_CODE, -1, NULL))
{
PLAYER_ERROR("Could not read aio ID ");
SetError(-1);
return;
}
if (AddInterface(m_aio_addr))
{
PLAYER_ERROR("Could not add aio interface ");
SetError(-1);
return;
}
// Create power interface
if (cf->ReadDeviceAddr(&m_power_addr, section,
"provides", PLAYER_POWER_CODE, -1, NULL))
{
PLAYER_ERROR("could not read power ID ");
SetError(-1);
return;
}
if (AddInterface(m_power_addr))
{
PLAYER_ERROR("could not add power interface");
SetError(-1);
return;
}
m_baud_rate = cf->ReadInt(section, "baud", 38400);
m_width = cf->ReadFloat(section, "width", 0.2);
m_length = cf->ReadFloat(section, "length", 0.2);
m_height = cf->ReadFloat(section, "height", 0.1);
m_quiet = cf->ReadBool(section, "quiet", TRUE);
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device. Return 0 if things go well, and -1 otherwise.
#ifdef PLAYER3
int WC132Driver::MainSetup()
#else
int WC132Driver::Setup()
#endif
{
PLAYER_MSG0(0, "wc132driver initialising\n");
// instantiate the interface library; this will find all active I2C master devices and serial ports
NdiCmd::Setup(NULL);
pNdiCmd = NdiCmd::NdiCmdFactory();
if (!pNdiCmd)
{
PLAYER_ERROR("Could not open NdiCmd library ");
return -1;
}
if (!pNdiCmd->GetBackplane())
{
PLAYER_ERROR("No backplane");
return -1;
}
// override baud rate on all parts to match requested one, if specified; otherwise, use defaults
if (0) // ->PETE m_baud_rate != -1)
{
int num_ports = pNdiCmd->GetBackplane()->GetNumSerialPorts();
for (int i = 0; i < num_ports; i++)
{
pNdiCmd->GetBackplane()->SetBaud(i, m_baud_rate);
}
}
// now search for our device
PLAYER_MSG0(0, "wc132driver searching for WC-132 devices...\n");
pNdiCmd->DoSearch();
if (pNdiCmd->GetNumWheelCommanders() == 0)
{
printf("wc132driver: could not find a device!\n");
//delete pNdiCmd;
pNdiCmd = NULL;
PLAYER_ERROR("Could not find a WC-132!");
return -1;
}
pWC = pNdiCmd->GetWheelCommander();
m_timestamp_ms = System::currentTimeMillis();
PLAYER_MSG0(0, "wc132driver ready\n");
pNdiCmd->LogQuiet(m_quiet);
// Start the device thread; spawns a new thread and executes
// WC132Driver::Main(), which contains the main loop for the driver.
StartThread();
return(0);
}
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
#ifdef PLAYER3
void WC132Driver::MainQuit()
#else
int WC132Driver::Shutdown()
#endif
{
PLAYER_MSG0(0, "Shutting wc132driver down\n");
// Stop and join the driver thread
StopThread();
// delete the interface library
if (pNdiCmd)
{
if (pWC)
{
PLAYER_MSG0(0, "Stopping motors.\n");
pWC->Coast();
}
pWC = NULL;
pNdiCmd->CloseAllPorts();
pNdiCmd = NULL;
}
PLAYER_MSG0(0, "wc132driver has been shutdown\n");
#ifndef PLAYER3
return(0);
#endif
}
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void WC132Driver::Main()
{
// The main loop; interact with the device here
for (;;)
{
// test if we are supposed to cancel
pthread_testcancel();
// process incoming messages; calls ProcessMessage() on each pending message
ProcessMessages();
// poll state of WC-132
UpdateData();
// give robot a chance to change state
usleep(10000);
}
if (pWC)
{
PLAYER_MSG0(0, "Stopping motors.\n");
pWC->Coast();
}
return;
}
int WC132Driver::ProcessMessage(QueuePointer & resp_queue,
player_msghdr * hdr,
void * data)
{
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_POS, m_position_addr))
{
assert(hdr->size == sizeof(player_position2d_cmd_pos_t));
ProcessPos2dPosCmd(hdr, *reinterpret_cast<player_position2d_cmd_pos_t *>(data));
return(0);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL, m_position_addr))
{
assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
ProcessPos2dVelCmd(hdr, *reinterpret_cast<player_position2d_cmd_vel_t *>(data));
return(0);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_DIO_CMD_VALUES, m_dio_addr))
{
ProcessDioCommand(hdr, *reinterpret_cast<player_dio_cmd_t *>(data));
return(0);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_DIO_DATA_VALUES, m_dio_addr))
{
ProcessDioData(hdr, *reinterpret_cast<player_dio_data_t *>(data));
return(0);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_AIO_DATA_STATE, m_aio_addr))
{
ProcessAioData(hdr, *reinterpret_cast<player_aio_data_t *>(data));
return(0);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_MOTOR_POWER, m_position_addr))
{
this->Publish(m_position_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
return 0;
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM, m_position_addr))
{
ProcessPos2dGeomReq(hdr);
return(0);
}
// Tell the caller that you don't know how to handle this message
return(-1);
}
void WC132Driver::UpdateData(void)
{
//
// Send out new messages with Driver::Publish()
//
int vel;
int rot;
//long int dist;
//int ang;
int new_timestamp_ms;
float dt;
float dx;
float dy;
float dtheta;
float v;
float w;
float delta_x;
float delta_y;
float delta_theta;
float theta;
new_timestamp_ms = System::currentTimeMillis();
dt = (new_timestamp_ms - m_timestamp_ms) / 1000.0;
m_timestamp_ms = new_timestamp_ms;
// query robot's current data
pWC->GetVelocity(vel, TRUE, TIMEOUT_MS);
pWC->GetRotation(rot, TRUE, TIMEOUT_MS);
//pWC->GetDistance(dist);
//pWC->GetAngle(ang);
theta = m_pos_data.pos.pa; // our only knowledge of our angle in the global reference frame
// calculate velocity and rotation rate of robot in meters and meters/sec
v = vel * VEL_CONVERSION_FACTOR;
w = rot * ROT_CONVERSION_FACTOR;
//PLAYER_MSG3(1, "vel rot deltat %f,%f,%f", v, w, dt);
// calculate velocity and rotation rate in global reference frame
dx = v * cos(theta);
dy = v * sin(theta);
dtheta = w;
// calculate change in pose in global reference frame
delta_x = dx * dt;
delta_y = dy * dt;
delta_theta = dtheta * dt;
theta += delta_theta;
theta = NORMALIZE(theta);
if (theta < 0)
theta += 2 * M_PI;
// finally, calculate actual pose in global reference frame
m_pos_data.pos.px += delta_x;
m_pos_data.pos.py += delta_y;
m_pos_data.pos.pa = theta;
// also report velocities
m_pos_data.vel.px = dx;
m_pos_data.vel.py = dy;
m_pos_data.vel.pa = dtheta;
m_pos_data.stall = 0;
//PLAYER_MSG3(1, "m_pos_data.pos %f,%f:%f", m_pos_data.pos.px, m_pos_data.pos.py, m_pos_data.pos.pa);
//PLAYER_MSG3(1, "m_pos_data.vel %f,%f:%f", m_pos_data.vel.px, m_pos_data.vel.py, m_pos_data.vel.pa);
Publish(m_position_addr,
PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
(void*)&m_pos_data, sizeof(m_pos_data), NULL);
double volts;
pWC->ReadServoSupply(volts);
//PLAYER_MSG1(1, "motor volts %g", volts);
memset(&m_power_data, 0, sizeof(m_power_data));
m_power_data.valid = PLAYER_POWER_MASK_VOLTS;
m_power_data.volts = volts;
Publish(m_power_addr,
PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE,
reinterpret_cast<void*>(&m_power_data), sizeof(m_power_data), NULL);
}
void WC132Driver::ProcessPos2dPosCmd(player_msghdr_t* hdr,
player_position2d_cmd_pos_t &data)
{
// PLS: implement
}
void WC132Driver::ProcessPos2dVelCmd(player_msghdr_t* hdr,
player_position2d_cmd_vel_t &data)
{
int vel;
int rot;
float v;
PLAYER_MSG3(1, "vel cmd %f,%f:%f", data.vel.px, data.vel.py, data.vel.pa);
v = sqrt(data.vel.px * data.vel.px + data.vel.py * data.vel.py);
vel = (int)(v / VEL_CONVERSION_FACTOR);
rot = (int)(data.vel.pa / ROT_CONVERSION_FACTOR);
PLAYER_MSG2(1, "sending vel=%d, rot=%d", vel, rot);
pWC->SetVelocity(vel);
pWC->SetRotation(rot);
pWC->Go();
}
void WC132Driver::ProcessDioCommand(player_msghdr_t* hdr,
player_dio_cmd_t &data)
{
unsigned int i;
for (i = 0; i < data.count; i++)
pWC->WriteDIO(i, (data.digout & (1 << i)) != 0);
Publish(m_dio_addr,
PLAYER_MSGTYPE_DATA, PLAYER_DIO_CMD_VALUES,
reinterpret_cast<void*>(&data), sizeof(data), NULL);
}
void WC132Driver::ProcessDioData(player_msghdr_t* hdr,
player_dio_data_t &data)
{
int bits;
BOOL bit;
int i;
for (i = 0, bits = 0; i < 8; i++)
{
if (pWC->ReadDIO(i, bit))
bits |= bit << i;
}
data.count = 8;
data.bits = bits;
Publish(m_dio_addr,
PLAYER_MSGTYPE_DATA, PLAYER_DIO_DATA_VALUES,
reinterpret_cast<void*>(&data), sizeof(data), NULL);
}
void WC132Driver::ProcessAioData(player_msghdr_t* hdr,
player_aio_data_t &data)
{
int i;
double v;
for (i = 0; i < 8; i++)
{
pWC->ReadAIO(i, v);
voltages[i] = (float)v;
}
data.voltages_count = 8;
data.voltages = voltages;
Publish(m_aio_addr,
PLAYER_MSGTYPE_DATA, PLAYER_AIO_DATA_STATE,
reinterpret_cast<void*>(&data), sizeof(data), NULL);
}
void WC132Driver::ProcessPos2dGeomReq(player_msghdr_t* hdr)
{
player_position2d_geom_t geom;
geom.pose.px = m_pos_data.pos.px; // [m]
geom.pose.py = m_pos_data.pos.py; // [m]
geom.pose.pz = m_pos_data.pos.pa; // [rad]
geom.size.sl = m_length; // [m]
geom.size.sw = m_width; // [m]
geom.size.sh = m_height; // [m]
Publish(m_position_addr,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM,
&geom, sizeof(geom), NULL);
}
wc132.cfg
# Instantiate the driver
driver
(
name "wc132"
plugin "libwc132_driver"
provides ["position2d:0" "dio:0" "aio:0" "power:0"]
baud 115200
)
wander.cpp
/************************************************************************/
/* Wander */
/* WheelCommander Player Example */
/* */
/* Based on the wander example from Jennifer Owen's excellent document */
/* "How to Use Player/Stage," 2nd Edition, page 55. */
/* */
/* http://www-users.cs.york.ac.uk/~jowen/player/playerstage-manual.html */
/* See also: */
/* http://playerstage.sourceforge.net/wiki/Tutorials */
/* */
/* version 1.0, 12/30/2010 */
/************************************************************************/
#include <stdio.h>
#ifndef _WIN32
#include <unistd.h>
#include <termios.h>
#else
#include <conio.h>
#endif
#include <time.h>
#include <sys/types.h>
#include <sys/time.h>
#include <libplayerc++/playerc++.h>
using namespace PlayerCc;
void Wander(double *forwardSpeed, double *turnSpeed)
{
int maxSpeed = 1;
int maxTurn = 180;
double fspeed, tspeed;
fspeed = rand() % 11;
fspeed = (fspeed / 10) * maxSpeed;
tspeed = rand() % (2 * maxTurn);
tspeed = tspeed - maxTurn;
*forwardSpeed = fspeed;
*turnSpeed = tspeed;
}
#ifndef _WIN32
// provide a way on Linux and Mac to detect a keypress and abort wandering...
void init(void)
{
changemode(1);
}
void done(void)
{
changemode(0);
}
void changemode(int dir)
{
static struct termios oldt, newt;
if ( dir == 1 )
{
tcgetattr( STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~( ICANON | ECHO );
tcsetattr( STDIN_FILENO, TCSANOW, &newt);
}
else
tcsetattr( STDIN_FILENO, TCSANOW, &oldt);
}
int _kbhit (void)
{
struct timeval tv;
fd_set rdfs;
tv.tv_sec = 0;
tv.tv_usec = 0;
FD_ZERO(&rdfs);
FD_SET (STDIN_FILENO, &rdfs);
select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);
return FD_ISSET(STDIN_FILENO, &rdfs);
}
#else
void init(void)
{
}
void done(void)
{
}
#endif
int main(int argc, char *argv[])
{
printf("Player Wander Demo v1.1\n\nStarting up...\n");
init();
PlayerClient robot("localhost", 6665);
Position2dProxy robot_drive(&robot, 0);
double forwardSpeed, turnSpeed;
srand(time(NULL));
robot_drive.SetMotorEnable(1);
//robot_drive.RequestGeom();
while(!_kbhit())
{
robot.Read();
Wander(&forwardSpeed, &turnSpeed);
robot_drive.SetSpeed(forwardSpeed, dtor(turnSpeed));
sleep(2);
}
robot_drive.SetMotorEnable(0);
printf("Player Wander Demo: done!\n");
done();
return 0;
}
© 2004-2011 Noetic Design, Inc. All Rights Reserved. Nubotics and WheelWatcher are trademarks of Noetic Design, Inc.