Caltech MVWT API
Written by Jon Gibbs ([email protected]) and David Tung ([email protected])
Introduction
This API is intended for programmers to control robots on the Caltech MVWT testbed. It
is derived from the code written by Steve Waydo, Bob Christie, and by the Cornell
Roboflag team. It contains documentation, an example program, and all necessary
header files to create a new program to interact with the testbed. The header files include
a point to point controller for the Steelebots. There are also troubleshooting notes in this
document.
Section 1: Motivation
Section 2: What your program needs to have
Section 3: Send/Receive Data Structures
Section 4: Communication Class and Functions
Section 5: Control Functions
Section 6: Program Flow: The Example Program
Section 7: MVServer
Section 1:
Motivation:
The Caltech Multi-Vehicle Wireless Testbed is an experimental platform for validating
theoretical advances in multiple-vehicle coordination and cooperation, real-time
networked control system, and distributed computation [1]. Before this API existed, the
testbed was only equipped to run the Cornell RoboFlag game. Programmers who wanted
to write custom code had to ether write onboard controllers for each robot or go through
the RoboFlag architecture:
Custom
Code
Team 1
Network
GUI
Master
GUI
Arbiter
MVServer
Team 2
Network
Custom
Code
GUI
Vision
System
Hardware Test bed
Writing code not intended for the RoboFlag game through the RoboFlag architecture is
inefficient and problematic. The biggest problem is that sometimes the arbiter may
override custom code because it enforces the rules of the RoboFlag game. Writing
onboard controllers is extremely tedious and some of the robots have limited computation
power. This API is intended to address these issues and meets the following objectives:
1) Create interface for non-RoboFlag programmers to control multiple robots without
writing onboard code. 2) Provide ability to control different types of Robots
simultaneously. 3) Provide basic point to point control.
The API has a new architecture that is easier to use for non-RoboFlag projects:
Custom Code
API
Functions
MVServer
Hardware Test bed
Vision
System
Section 2:
What your program needs to have:
This API is intended for users who want to write Windows programs to control robots in
the testbed. There are two libraries and one header file that one needs in order to be able
to communicate with MVServer: Ws2_32.lib and roboflagR.lib. Ws2_32.lib can be
included in a VisualC++ 6 workspace by following these steps (Note: The library can be
obtained from Microsoft’s online MSDN library):
1. Right click on the project and then select settings:
2. Select the “Link” tab and add Ws2_32.lib to the library modules field under then
click OK.
Note: Ws2_32.lib is a library for basic windows socket programming. This API uses an
object defined in this library for communication with MVServer.
The second library needed is roboflagR.lib. It should be placed in your project folder.
Add it by creating a new folder in your workspace. Right click that folder and select
“Add Files to Folder”. Select roboflagR.lib and you should be set.
The header file “SendRecieve.h” included in this API must also be added. You should
add it to the “Header Files” folder within your project following the same steps you used
to add the roboflagR library.
Section 3:
Send/Receive Data Structures
Currently there are data structures that your program must use in order to communicate
with MVServer. Note: If you wish to change these structures, see Section 7.
The send structure:
struct ObjectVelocity
{
float xVel, yVel, rotVel;
};
struct EntityCommands:ObjectVelocity
{
int kick, dribble;
};
struct ObjectCommands
{
EntityCommands robots[NUM_OF_TEAMS*NUM_OF_ROBOTS],
obstacles[NUM_OF_OBSTACLES];
};
ObjectCommands is the data structure your program will send to MVServer which will
send it to the robots. The constants NUM_OF_TEAMS, NUM_OF_ROBOTS, and
NUM_OF_OBSTACLES are defined inside of the SendRecieve.h file and the time of
writing were consistent with the numbers defined inside of the MVServer program. If the
numbers are not consistent, MVServer will print an error message similar to ERROR:
packets dropped, size = 40.
Troubleshooting: If the robots are not responding and MVServer displays an error
message similar to: ERROR: packets dropped, size = 40, then the constants
NUM_OF_TEAMS, NUM_OF_ROBOTS, and NUM_OF_OBSTACLES are not
consistent with what is defined inside the MVServer program. In order for commands to
get to the robots, the constants must be the same across MVServer and in SendReceive.h,
even if part of the array is not used.
The receive data structure:
struct LocationCommon
{
float xPos, yPos, rotation;
};
struct MovingObject:LocationCommon
{
bool isFound, isRotationFound;
};
struct RawVision
{
MovingObject robots[NUM_OF_TEAMS*NUM_OF_ROBOTS],
balls[NUM_OF_BALLS],
obstacles[NUM_OF_OBSTACLES];
float time;
};
RawVision is the structure that your program will receive from MVServer. It contains
position information members (xPos, yPos, rotation), for each robot in the robots array.
Section 4:
Communication Class and Functions
1) Class: Client
The client class is used to create a connection with the MVServer and communicate with
it.
Class Constructor:
Client(CommType OurclientType, const char* hostname, int port);
Arguments:
CommType OurclientType: set this to SEND_RECEIVE to both send and receive data
const char* hostname: set this to the IP address of the MVServer
int port: set this to the MVServer port number
Example:
int MVserverPort = 4545;
char MVserverIP[16] = "192.168.1.231";
Client * client = new Client(SEND_RECEIVE, MVserverIP, MVserverPort);
2) Function: openConnection
The openConnection function is a member of the Client class and it opens the connection
with the MVServer.
Function Prototype:
bool openConnection( );
Return Value:
True is connection successfully established, false is could not connect to the MVServer.
Example:
if(!client->openConnection())
{
cerr << "ERROR: Could not connect to the Server" << endl;
return 1;
}
3) Function: receive
The receive function is a member of the Client class and it receives initial data from the
MVServer.
Function prototype:
bool receive( void* message, int size );
Arguments:
void* message: the data received from MVServer
int size: the size of the data received from MVServer
Returns Value:
True if it successfully received data from MVServer, false if it did not successfully
receive data from MVServer.
Example:
if(!client->receive(&visionData, sizeof(visionData)))
{
cerr << "ERROR: Could not receive initial position data from the Server" << endl;
return 1;
}
4) Function: receiveLatest
The receive function is a member of the Client class and it receives data from the
MVServer.
Function prototype:
bool receiveLatest(void *msg, int msgsize);
Arguments:
void* msg: the data received from MVServer
int msgsize: the size of the data received from MVServer
Returns Value:
True if it successfully received data from MVServer, false if it did not successfully
receive data from MVServer.
Example:
if(!client->receiveLatest(&visionData, sizeof(visionData)))
{
cerr << "ERROR: Could not receive data from the Server" << endl;
return 1;
}
5) Function: send
The send function is a member of the Client class and it sends commands to the
MVServer.
Function Prototype:
bool send( void* message, int size );
Arguments:
void* message: the data sent to the MVServer
int size: the size of the data sent to the MVServer
Return Value:
True if it successfully sent data to MVServer, false if it did not successfully send data to
MVServer.
Example:
client->send(&objectCommands, sizeof(objectCommands));
Section 5:
Control Functions
These functions provide direct velocity control and point to point control. They have
been tested to work smoothly and accurately while RFController is running on the
Steelebots.
Velocity Control
1) Function: setVelocityCommand
Function Prototype:
ObjectCommands setVelocityCommand(float xVel, float yVel, int robotNum,
ObjectCommands myObjectCommand);
This function allows the programmer to set the x-velocity and y-velocity of a robot.
Arguments:
float xVel: x-velocity command
float yVel: y-velocity command
int robotNum: Robot number
ObjectCommands myObjectCommand: ObjectCommands structure that is updated
Return Value:
ObjectCommands structure that was passed to it as an argument with updated x-velocity
and y-velocity commands for the specified robot.
Example:
ObjectCommands objectCommands;
objectCommands = setVelocityCommand(xVelCmd[i], yVelCmd[i], i,
objectCommands);
Point to Point Control
Two functions made available to users to control robots from point to point:
• getErrors: obtain errors between current position and target position
• P2PvelocityCommand: given these errors, obtain velocity commands to achieve
target position
1) Function: getErrors
This function provides x-position and y-position error between robot’s current position
and target position
Function Prototype:
LocationCommon getErrors(RawVision visionInfo, float x_target, float y_target, int
robotNum);
Arguments:
RawVision visionInfo: RawVision structure containing position of robots
float x_target: Target x-position
float y_target: Target y-position
int robotNum: Robot number
Return Value:
LocationCommon structure with x-position error and y-position error for specified robot.
Example:
LocationCommon Errors = getErrors(visionData, x_ref, y_ref, i);
2) Function: P2PvelocityCommand
Function Prototype:
ObjectCommands P2PvelocityCommand(LocationCommon CurrentDeltas, int
robotNum, ObjectCommands myObjectCommand);
This function provides velocity commands to achieve a target position. It is a
proportional controller that sets x-velocity and y-velocity commands proportional to xposition error and y-position error.
Arguments:
LocationCommon CurrentDeltas: LocationCommons structure with position errors
obtained in getErrors function
int robotNum: Robot number
ObjectCommands myObjectCommand: ObjectCommands structure
Return Value:
ObjectCommands structure that was passed to it as an argument with updated x-velocity
and y-velocity commands for the specified robot to make it compensate for position error.
Example:
objectCommands = P2PvelocityCommand(Errors, i, objectCommands);
Section 6:
Program Flow: The Example Program
This section is devoted to program flow. There are a couple constraints on the order that
certain functions should be called and how often they are called.
The basic order of the example program is as follows:
1) Create the object of the client class:
Client * client = new Client(SEND_RECEIVE, MVserverIP,
MVserverPort);
2) Open connection with MVServer:
if(!client->openConnection())
3) Declare RawVision and ObjectCommands structures then receive initial position
data from MVServer.
ObjectCommands objectCommands;
RawVision visionData;
// receive initial positions for objects
if(!client->receive(&visionData, sizeof(visionData)))
4) Begin the while loop. First the position data from the server is received. The
ObjectCommands structure is updated for each vehicle and is then sent back to
MVServer. Note: It is important that no keyboard inputs are taken at every
iteration inside this loop because it needs to run at a high frequency.
if(!client->receiveLatest(&visionData, sizeof(visionData)))
Note: The difference is that now you must call the receivelatest
function instead of the receive function that was used to get
initial information
client->send(&objectCommands, sizeof(objectCommands));
After the while loop finishes, the connection should be closed.
Section 7:
MVServer
The MVServer must run on Linux. Below is the documentation for the
init_arbiter_connection() function in the MVServer code, which opens a connection with
your Windows program (referred to as arbiter in MVServer).
Function: init_arbiter_connection()
1) To create the socket for the arbiter connection socket(AF_INET, SOCK_STREAM, 0)
is called. If it returns -1 there is an error.
2) Set the Arbiter listener for reuse by calling the function setsockopt(listen_fd,
SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(int)). If this function returns -1, there
is an error.
3) Sets up the socket address that we bind the socket to. We specify the type of socket,
the IP used, and the port used. AF_INET is a type of socket. The port is a parameter.
htons and htonl are library functions.
struct sockaddr_in sa;
memset(&sa, 0, sizeof(sa));
sa.sin_family = AF_INET;
sa.sin_port = htons(port);
sa.sin_addr.s_addr = htonl(INADDR_ANY);
4) The bind function attempts associate that socket with a port on the local machine.
bind(listen_fd, (struct sockaddr *)&sa, sizeof(sa))
5) We listen for a connection and take only the first one specified by the second
parameter. listen_fd is the integer file descriptor returned by the socket function earlier
in the code.
listen(listen_fd, 1)
6) FD_ZERO clears a set of file descriptors. FD_SET will add a given descriptor to the
set of file descriptors.
7) We finally wait for the arbiter to connect. It uses a timeout to wait for the connection
select(MAX(listen_fd, STDIN_FILENO) + 1, &readfds, NULL, NULL, NULL)
If this function returns -1, there is an error.
8) FD_ISSET tests to see if a descriptor is part of the set. We don’t know what the
significance of putting a file descriptor in the set is. If the user presses any key, the select
function will time out and we think that STDIN_FILENO becomes mysteriously part of
the set. We think that STDIN_FILENO becomes a part of the set if there is any keyboard
input in the console. If there is any keyboard input the read function empties up to
DATA_BUFFSIZE bytes of the stdin buffer.
read(STDIN_FILENO, &data_buffer, DATA_BUFFSIZE)
9) Finally the accept function accepts a connection on the socket.
sock_fd = accept(listen_fd, (struct sockaddr *)&arbiter_addr, &aa_size)). If sock_fd
equals -1, then there is an error.
10) The variable delay is a parameter that decides whether or not to use Nagle’s
algorithm. Nagle’s algorithm accumulates small packets into one large for better
efficiency in a congested network.
11) The function setsockopt is manipulating the Nagle’s algorithm option. In this case, it
is disabling Nagle’s algorithm.
setsockopt(sock_fd, IPPROTO_TCP, TCP_NODELAY, (int *)&dflag, sizeof(int))
12) Last we must do the arbiter handshake. For the handshake protocol, it receives a
message from the arbiter and then sends a message in return. After this handshake then
the socket variable arbiter_sock is set to sock_fd and the function is finished.
Note: In order to change the Send/Receive data structures, the user must modify
datatypes.h and compile it with MVServer. You must then put that same data structure in
“SendReceive.h” and then compile your new program. Make sure that the size of the
arrays in both header files is the same too.
Reference:
[1] Z. Jin, S. Waydo, E. Wildanger, M. Lammers, H. Scholze, P. Foley, D. Held, and R.
Murray. MVWT-II: The Second Generation Caltech Multi-Vehicle Wireless