int main(int argc, char **argv)
{
if(argc < 3)
{
std::cout << "Usage: " << argv[0] << " <vicon_server> <calib_files_dir>" << std::endl;
return 1;
}
const std::string vicon_hostname = std::string(argv[1]);
calib_files_dir = std::string(argv[2]);
if(IPC_connect("vicon") != IPC_OK)
{
std::cerr << "Error connecting to IPC" << std::endl;
return 1;
}
ViconDriver vd;
if(vd.init(vicon_hostname))
{
vd.setSubjectPubCallback(subject_publish_callback);
vd.setUnlabeledMarkersPubCallback(unlabeled_markers_publish_callback);
vd.enableUnlabeledMarkerData(true);
running = vd.start();
}
else
{
std::cerr << "Error connecting to vicon server" << std::endl;
}
if(running)
{
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = sigint_handler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
}
while(running)
{
std::cout << "Running: " << running << std::endl;
if(pthread_mutex_trylock(&ipc_mutex) == 0)
{
IPC_handleMessage(0);
pthread_mutex_unlock(&ipc_mutex);
}
sleep(1);
}
std::cout << "Shutting down ViconDriver" << std::endl;
vd.shutdown();
std::cout << "Disconnecting from IPC server" << std::endl;
IPC_disconnect();
return 0;
}
int main (void)
#endif
{
int j;
float *data;
#ifdef VXWORKS
{ int currentPriority;
/* The producer needs to run at a lower priority, else it swamps
central and the consumer */
taskPriorityGet(taskIdSelf(), ¤tPriority);
taskPrioritySet(taskIdSelf(), currentPriority+10);
}
#endif
data = (float *)malloc(MAX_DATA_SIZE);
for (j = 0; j<MAX_DATA_SIZE/sizeof(float); j++)
data[j] = (float) j;
IPC_connect("sender");
IPC_defineMsg(NULL_MSG, 0, NULL);
IPC_defineMsg(FLOAT4_MSG, sizeof(float)*4, NULL);
IPC_defineMsg(FLOAT16_MSG, sizeof(float)*16, NULL);
IPC_defineMsg(FLOAT64_MSG, sizeof(float)*64, NULL);
IPC_defineMsg(FLOAT256_MSG, sizeof(float)*256, NULL);
IPC_defineMsg(FLOAT1K_MSG, sizeof(float)*1024, NULL);
IPC_defineMsg(FLOAT4K_MSG, sizeof(float)*4*1024, NULL);
IPC_defineMsg(FLOAT16K_MSG, sizeof(float)*16*1024, NULL);
IPC_defineMsg(FLOAT64K_MSG, sizeof(float)*64*1024, NULL);
IPC_defineMsg(FLOAT256K_MSG, sizeof(float)*256*1024, NULL);
IPC_subscribe(DONE_MSG, doneHandler, NULL);
sendMessages(NULL_MSG, 0, data);
sendMessages(FLOAT4_MSG, sizeof(float)*4, data);
sendMessages(FLOAT16_MSG, sizeof(float)*16, data);
sendMessages(FLOAT64_MSG, sizeof(float)*64, data);
sendMessages(FLOAT256_MSG, sizeof(float)*256, data);
sendMessages(FLOAT1K_MSG, sizeof(float)*1024, data);
sendMessages(FLOAT4K_MSG, sizeof(float)*4*1024, data);
sendMessages(FLOAT16K_MSG, sizeof(float)*16*1024, data);
sendMessages(FLOAT64K_MSG, sizeof(float)*64*1024, data);
sendMessages(FLOAT256K_MSG, sizeof(float)*256*1024, data);
/* Wait for everything to settle down before shutting down */
sleep(2);
/* shut down */
printf ("\n");
fflush(stdout);
IPC_disconnect();
#if !defined(VXWORKS)
return 0;
#endif
}
int main(int argc, char* argv[])
{
gpsHeading = 0;
compassHeading = 0;
compassYawRate = 0;
yawRate = 0;
gpsSpeed = 0;
leftWheelSpeed = 0;
rightWheelSpeed = 0;
QueryPerformanceFrequency(&frequency);
QueryPerformanceCounter(¤tTick);
//Connect to IPC
IPC_connectModule("StateEstimationTest","localhost");
time_t theTime;
time(&theTime);
tm *localTime = localtime(&theTime);
char filename[1000];
sprintf(filename, LOGGING_ROOT);
sprintf(filename + strlen(LOGGING_ROOT), "%04d-%02d-%02d-%02d-%02d-%02d-SEtest.csv", localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday, localTime->tm_hour, localTime->tm_min, localTime->tm_sec);
file.open(filename);
file << "GPS Speed,GPS Heading,Compass Heading,Compass Yaw Rate,Gyro Yaw Rate,LeftWheel,RightWheel" << endl;
try
{
//Subscribe to GPS and wheel speed
Messages::StateEstimation.subscribe(StateEstHandler,true,false);
Messages::GPSUpdate.subscribe(GPSHandler,true,false);
Messages::CompassUpdate.subscribe(CompassHandler,true,false);
Messages::GyroUpdate.subscribe(GyroHandler,true,false);
Messages::GetWheelSpeed.subscribe(WheelSpeedHandler,true,false);
Timer::addTimer(tick, INPUT_TICK);
IPC_dispatch();
}
catch( exception &e)
{
cout << e.what() << endl;
}
catch(...)
{
cout << "non-exception error" << endl;
}
// teardown
IPC_disconnect();
return 0;
}
void *test_central_thread(void *ptr)
{
carmen_central_p central;
int err;
central = (carmen_central_p)ptr;
do {
if(central->connected || central->ready_for_reconnect)
usleep(250000);
else {
err = carmen_multicentral_ipc_connect(module_name, central->host);
if(err >= 0) {
IPC_disconnect();
central->ready_for_reconnect = 1;
}
}
} while(1);
return NULL;
}
/* Echo the input. In addition,
* typing "q" will quit the program;
* typing "m" will send another message;
* typing "u" will unsubscribe the handler (the program will no
* longer listen to input)
*/
static void stdinHnd (int fd, void *clientData)
{
char inputLine[81];
fgets(inputLine, 80, stdin);
printf("stdinHnd [%s]: Received %s", (char *)clientData, inputLine);
fflush(stdout);
if (strlen(inputLine) == 2) { /* Only one character and the newline */
if (inputLine[0] == 'q') {
IPC_disconnect();
exit(0);
} else if (inputLine[0] == 'm') {
char *string = "Forwarding";
/* Need to make sure the trailing NULL character is sent */
printf("\n IPC_publish(%s, %d, %s)\n",
MSG2, (int)strlen(string)+1, string);
IPC_publish(MSG2, strlen(string)+1, string);
} else if (inputLine[0] == 'u') {
IPC_unsubscribeFD(fd, stdinHnd);
}
}
}
JNIEXPORT jint JNICALL Java_ipc_java_IPC_IPC_1disconnect (JNIEnv *env, jclass theClass)
{
return (jint)IPC_disconnect();
}
int main(int argc, const char *argv[]) {
// connect to IPC
int quit = 0;
int connection_status = init_ipc("darwin_command_ipc",argv[1]);
// subscribe to relevant IPC messages
IPC_subscribeData(SVM_DATA_RESPONSE, svmDataHandler, NULL); // ball data
// listen to robot information messages
// listen to messages sent by other robots
printf("Start main loop.\n");
// send messages to darwin-nav on own robot
Darwin_Nav_Command command;
int publish_command = 0; // set to 0 for no, 1 to send out command next loop
double t0 = ftime(); //start timer for find_ball in demo state machine
State state = FIND_BALL; //TODO: sharing states between machines is very bad :(
State state_find_ball = TURN_HEAD_SIDE_TO_SIDE;
State state_chase_ball = BALL_CENTERED;
// start walking (NOTE: arrange so this is not necessary TODO: may want to remove)
command.mode = DARWIN_NAV_START;
IPC_publishData(DARWIN_NAV_COMMAND, &command);
while( quit == 0 ) {
//print_svm_data();
// check IPC connection status
// break if we're not connected for now
//TODO: reconnect or wait? Or, only send while connected?
if( connection_status != 0 ) {
printf( "IPC connection disrupted, disconnecting...\n"); //TODO: realtime
break;
}
IPC_listenClear(10); //listen for messages
// run demo state machine here...
search_and_chase_ball(&publish_command,
&command,
&t0,
&state,
&state_find_ball,
&state_chase_ball);
// comment out search_and_chase_ball to try this other state machine:
/* turn_head_towards_ball(&publish_command,
&command,
&state_chase_ball);*/
// if we have a new command to publish, do so
if( publish_command == 1 ) {
IPC_publishData(DARWIN_NAV_COMMAND, &command);
publish_command = 0;
}
// TODO: any state machine in a function that wants to send nav commands
// needs to take in &command_changed and &command
/* state machine stuff
// decide role
Role role = ATTACKER;
// things that decide role:
// ball distance
// if we're the goalie or not
// is another robot in a better position?
// are we trying to pass?
// state machine
switch( role ) {
}
Mode mode = FIND_BALL;
// psedocode for finding, then kicking ball
if( ball_seen ) {
if( ball_within_reach ) {
if( kick_places_ball_near_goal ) {
kick_ball();
}
else {
move_ball_near_goal();
}
}
else {
turn_toward_ball();
forward();
}
}
else {
find_ball();
}
switch( mode ) {
// demo state machine:
// if we don't see the ball, find the ball
// else if we see the ball, track the ball
// follow and kick the ball:
// if we're near the ball and TODO ball kicked would be favorable...
// align self with ball (sidestep if necessary)
// kick ball
// else if we're not near the ball
// turn toward ball
// walk toward ball UNTIL
// (different mode: walk to the goalposts and align kick between?
//
case FIND_BALL:
break;
case FOUND_BALL:
break;
}*/
}// end main loop TODO: make more robust?
// disconnect from IPC after quitting...
IPC_disconnect();
}// end main //
void ipcTest1(void)
{
/* Actually send one more byte (end-of-string) */
#define FIVE_BYTE_MSG_LEN 6
#define SIX_BYTE_MSG_LEN 7
#define EIGHT_BYTE_MSG_LEN 9
char fiveBytes[FIVE_BYTE_MSG_LEN] = "abcde";
char sixBytes[SIX_BYTE_MSG_LEN] = "abcdef";
char eightBytes[EIGHT_BYTE_MSG_LEN] = "abcdefgh";
fiveBytes[FIVE_BYTE_MSG_LEN-1] = '\0';
sixBytes[SIX_BYTE_MSG_LEN-1] = '\0';
eightBytes[EIGHT_BYTE_MSG_LEN-1] = '\0';
/****************************************************************
* TESTS OF THE BASIC IPC INTERFACE
****************************************************************/
/* Connect to the central server */
printf("\nIPC_connect(%s)\n", TASK_NAME);
IPC_connect(TASK_NAME);
/* Default is to exit on error; Override default, because some of the
tests in this file explicitly induce errors. */
printf("\nIPC_setVerbosity(IPC_Print_Errors)\n");
IPC_setVerbosity(IPC_Print_Errors);
/* Test out the timers */
realStart = x_ipc_timeInMsecs();
printf("\nIPC_addTimer(1000, TRIGGER_FOREVER, timerHandler, \"timer1\")\n");
IPC_addTimer(1000, TRIGGER_FOREVER, (TIMER_HANDLER_TYPE)timerHandler,
(void *)"timer1");
/* Define a fixed length message (no format string) */
printf("\nIPC_defineMsg(%s, %d, NULL)\n", MSG1, FIVE_BYTE_MSG_LEN);
IPC_defineMsg(MSG1, FIVE_BYTE_MSG_LEN, NULL);
/* Define a variable length message (no format string) */
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, NULL)\n", MSG2);
IPC_defineMsg(MSG2, IPC_VARIABLE_LENGTH, NULL);
printf("\nIPC_isMsgDefined(%s) => %s\n",
MSG1, IPC_isMsgDefined(MSG1) ? "TRUE" : "FALSE");
printf("\nIPC_isMsgDefined(%s) => %s\n",
MSG3, IPC_isMsgDefined(MSG3) ? "TRUE" : "FALSE");
/* Subscribe to the first message, with client data */
printf("\nIPC_subscribe(%s, msg1Handler, %s)\n", MSG1, "client1a");
IPC_subscribe(MSG1, msg1Handler, (void *)"client1a");
/* Subscribe to the second message, with client data */
printf("\nIPC_subscribe(%s, msg1Handler, %s)\n", MSG2, "client2a");
IPC_subscribe(MSG2, msg1Handler, (void *)"client2a");
/* Publish the fixed length message and listen for it (in this simple
example program, both publisher and subscriber are in the same process */
printf("\nIPC_publish(%s, IPC_FIXED_LENGTH, %s)\n", MSG1, fiveBytes);
if (IPC_publish(MSG1, IPC_FIXED_LENGTH, fiveBytes) == IPC_OK)
/* MSECS is a simple macro that converts from seconds to msecs */
IPC_listenClear(MSECS(1));
/* Publish the fixed length message, giving the length explicitly */
/* Send one extra byte -- for end-of-string */
printf("\nIPC_publish(%s, %d, %s)\n", MSG1, FIVE_BYTE_MSG_LEN, fiveBytes);
if (IPC_publish(MSG1, FIVE_BYTE_MSG_LEN, fiveBytes) == IPC_OK)
IPC_listenClear(MSECS(1));
/* Produces an error, since the length passed is not the defined length */
printf("\nIPC_publish(%s, %d, %s)\n", MSG1, 10, fiveBytes);
if (IPC_publish(MSG1, 10, fiveBytes) == IPC_Error) printf("ERROR\n");
/* Publish the variable length message, sending (and receiving) 5 bytes */
/* Send one extra byte -- for end-of-string */
printf("\nIPC_publish(%s, %d, %s)\n", MSG2, FIVE_BYTE_MSG_LEN, fiveBytes);
if (IPC_publish(MSG2, FIVE_BYTE_MSG_LEN, fiveBytes) == IPC_OK)
IPC_listenClear(MSECS(1));
/* Publish the same variable length message, this time sending 8 bytes */
/* Send one extra byte -- for end-of-string */
printf("\nIPC_publish(%s, %d, %s)\n", MSG2, EIGHT_BYTE_MSG_LEN, eightBytes);
if (IPC_publish(MSG2, EIGHT_BYTE_MSG_LEN, eightBytes) == IPC_OK)
IPC_listenClear(MSECS(1));
/* Produces an error: Cannot pass IPC_VARIABLE_LENGTH as an argument */
printf("\nIPC_publish(%s, IPC_VARIABLE_LENGTH, %s)\n", MSG2, eightBytes);
if (IPC_publish(MSG2, IPC_VARIABLE_LENGTH, eightBytes) == IPC_Error)
printf("ERROR\n");
/* Produces an error: "msg2" is a variable length message */
printf("\nIPC_publish(%s, IPC_FIXED_LENGTH, %s)\n", MSG2, eightBytes);
if (IPC_publish(MSG2, IPC_FIXED_LENGTH, eightBytes) == IPC_Error)
printf("ERROR\n");
/* Get notified when handlers subscribe/unsubscribe to "msg2" */
printf("\n%d handlers currently subscribed to " MSG2, IPC_numHandlers(MSG2));
printf("\nIPC_subscribeHandlerChange("MSG2", handlerChangeHandler, NULL)\n");
IPC_subscribeHandlerChange(MSG2, handlerChangeHandler, NULL);
/* Subscribe a second message handler for "msg2" */
printf("\nIPC_subscribe(%s, msg2Handler, %s)\n", MSG2, "client2b");
IPC_subscribe(MSG2, msg2Handler, (void *)"client2b");
/* If doing direct broadcasts, need to listen to get the direct info update */
IPC_listen(250);
/* Publish the message -- receive two messages (one for msg1Handler,
one for msg2Handler). */
/* Send one extra byte -- for end-of-string */
printf("\nIPC_publish(%s, %d, %s)\n", MSG2, EIGHT_BYTE_MSG_LEN, eightBytes);
if (IPC_publish(MSG2, EIGHT_BYTE_MSG_LEN, eightBytes) == IPC_OK) {
/* Make sure all the subscribers get invoked before continuing on
(keep listening until a second has passed without any msgs) */
while (IPC_listen(MSECS(1)) != IPC_Timeout){};
}
/* Remove this subscription */
printf("\nIPC_unsubscribe(%s, msg2Handler)\n", MSG2);
IPC_unsubscribe(MSG2, msg2Handler);
/* If doing direct broadcasts, need to listen to get the direct info update */
IPC_listen(250);
/* No longer get notified when handlers are added/removed */
printf("\nIPC_unsubscribeHandlerChange("MSG2", handlerChangeHandler)\n");
IPC_unsubscribeHandlerChange(MSG2, handlerChangeHandler);
printf(" IPC_subscribe(%s, msg2Handler, %s)\n", MSG2, "client2c");
IPC_subscribe(MSG2, msg2Handler, (void *)"client2c");
printf(" IPC_unsubscribe(%s, msg2Handler)\n", MSG2);
IPC_unsubscribe(MSG2, msg2Handler);
/* Publish the message -- receive one message (for msg1Handler) */
printf("\nIPC_publish(%s, %d, %s)\n", MSG2, EIGHT_BYTE_MSG_LEN, eightBytes);
if (IPC_publish(MSG2, EIGHT_BYTE_MSG_LEN, eightBytes) == IPC_OK)
IPC_listenClear(MSECS(1));
/* Subscription of the same message handler *replaces* the old client data */
printf("\nIPC_subscribe(%s, msg1Handler, %s)\n", MSG1, "client1b");
IPC_subscribe(MSG1, msg1Handler, (void *)"client1b");
/* Receive one message (for msg1Handler), but now with new client data.
Note use of IPC_publishFixed. */
printf("\nIPC_publishFixed(%s, %s)\n", MSG1, fiveBytes);
if (IPC_publishFixed(MSG1, fiveBytes) == IPC_OK) IPC_listenClear(MSECS(1));
/* Remove subscription to "msg1" */
printf("\nIPC_unsubscribe(%s, msg1Handler)\n", MSG1);
IPC_unsubscribe(MSG1, msg1Handler);
/* If doing direct broadcasts, need to listen to get the direct info update */
IPC_listen(250);
/* Receive no messages -- IPC_listenClear times out */
printf("\nIPC_publishFixed(%s, %s)\n", MSG1, fiveBytes);
if (IPC_publishFixed(MSG1, fiveBytes) == IPC_OK)
if (IPC_listen(MSECS(1)) == IPC_Timeout) printf("Timed out\n");
/****************************************************************
* TESTS OF THE QUERY/RESPONSE FUNCTIONS
****************************************************************/
/* The handler of QUERY_MSG does two things: It *publishes* a message of
type MSG2, and it *responds* to the query with a message of type MSG2.
The published message gets handled only by the subscriber (msg1Handler),
and the response gets handled only be replyHandler, since a response
is a directed message. */
/* NOTE: It is perfectly OK to subscribe to a message before it is defined! */
printf("\nIPC_subscribe(%s, queryHandler, %s)\n", QUERY_MSG, "qtest");
IPC_subscribe(QUERY_MSG, queryHandler, (void *)"qtest");
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, NULL)\n", QUERY_MSG);
IPC_defineMsg(QUERY_MSG, IPC_VARIABLE_LENGTH, NULL);
/* This call allows IPC to send the process 2 messages at a time, rather than
queueing them in the central server.
This is needed in this example program because the sender and receiver of
the query are the same process. If this is taken out, the only difference
is that the message that is published in queryHandler arrives *after* the
message responded to (even though it is sent first). This function should
not be needed when we switch to using point-to-point communications
(rather than sending via the central server). */
IPC_setCapacity(2);
/* Send one extra byte -- for end-of-string */
printf("\nIPC_queryNotify(%s, %d, %s, replyHandler, %s)\n",
QUERY_MSG, SIX_BYTE_MSG_LEN, sixBytes, "Notification");
IPC_queryNotify(QUERY_MSG, SIX_BYTE_MSG_LEN, sixBytes, replyHandler,
(void *)"Notification");
/* Make sure all the messages spawned by this query get handled before
continuing (keep listening until a second has passed without any msgs) */
while (IPC_listen(MSECS(1)) != IPC_Timeout){};
{ char *replyHandle = NULL;
/* This essentially does the same thing as IPC_queryNotify above, except
it is blocking, and sets the replyHandle to be the data responded to.
Don't need to listen, since that is done within queryResponse, but
could be dangerous to wait forever (if the response never comes ...) */
/* Send one extra byte -- for end-of-string */
printf("\nIPC_queryResponse(%s, %d, %s, replyHandle, IPC_WAIT_FOREVER)\n",
QUERY_MSG, SIX_BYTE_MSG_LEN, sixBytes);
if (IPC_queryResponse(QUERY_MSG, SIX_BYTE_MSG_LEN, sixBytes,
(void **)(void *)&replyHandle,
IPC_WAIT_FOREVER) == IPC_OK) {
printf("Blocking Response: %s\n", replyHandle);
IPC_freeByteArray((void *)replyHandle);
}
/* This one should time out before the response arrives */
/* Send one extra byte -- for end-of-string */
printf("\nIPC_queryResponse(%s, %d, %s, replyHandle, %d)\n",
QUERY_MSG, SIX_BYTE_MSG_LEN, sixBytes, 0);
if (IPC_queryResponse(QUERY_MSG, SIX_BYTE_MSG_LEN, sixBytes,
(void **)(void *)&replyHandle, 0) == IPC_OK) {
printf("Blocking Response: %s\n", replyHandle);
IPC_freeByteArray((void *)replyHandle);
} else {
/* NOTE: Since the function call times out before handling messages,
* (a) The *response* to the query is lost (for good)
* (b) The message *published* in queryHandler is waiting for the next
* time the module listens for messages (which actually occurs
* in IPC_msgFormatter, below).
*/
printf("queryResponse timed out (replyHandle: %ld)\n", (long)replyHandle);
}
}
/****************************************************************
* TESTS OF THE MARSHALLING FUNCTIONS
****************************************************************/
/* Test the marshalling/unmarshalling functions, independently of
* sending/receiving messages
*/
{ IPC_VARCONTENT_TYPE varcontent;
SAMPLE_TYPE sample;
SAMPLE_PTR sample2Ptr;
sample.i1 = 666; sample.str1 = "hello, world"; sample.d1 = 3.14159;
printf("\nIPC_marshall(...)\n");
IPC_marshall(IPC_parseFormat(SAMPLE_FORMAT), &sample, &varcontent);
printf("Marshall of 'sample' [length: %d]\n", varcontent.length);
printf("\nIPC_unmarshall(...)\n");
IPC_unmarshall(IPC_parseFormat(SAMPLE_FORMAT),
varcontent.content, (void **)(void *)&sample2Ptr);
printf("Orig: <%d, %s, %f>\nCopy: <%d, %s, %f>\n",
sample.i1, sample.str1, sample.d1,
sample2Ptr->i1, sample2Ptr->str1, sample2Ptr->d1);
IPC_freeByteArray(varcontent.content);
IPC_freeData(IPC_parseFormat(SAMPLE_FORMAT), sample2Ptr);
}
{ IPC_VARCONTENT_TYPE varcontent;
MATRIX_LIST_TYPE m1, m2, m3;
int i, j, k;
char *string;
/* Define a variable-length message whose format is simply an integer */
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, %s)\n", MSG3, INT_FORMAT);
IPC_defineMsg(MSG3, IPC_VARIABLE_LENGTH, INT_FORMAT);
/* Define a variable-length message whose format is a string */
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, %s)\n",
MSG4, STRING_FORMAT);
IPC_defineMsg(MSG4, IPC_VARIABLE_LENGTH, STRING_FORMAT);
/* Define a variable-length message whose format is a complex structure */
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, %s)\n",
MSG5, MATRIX_LIST_FORMAT);
IPC_defineMsg(MSG5, IPC_VARIABLE_LENGTH, MATRIX_LIST_FORMAT);
/* Subscribe to each of the above messages, all using the same handler */
printf("\nIPC_subscribe(%s, msg3Handler, NULL)\n", MSG3);
IPC_subscribe(MSG3, msg3Handler, NULL);
printf("\nIPC_subscribe(%s, msg3Handler, NULL)\n", MSG4);
IPC_subscribe(MSG4, msg3Handler, NULL);
printf("\nIPC_subscribe(%s, msg3Handler, NULL)\n", MSG5);
IPC_subscribe(MSG5, msg3Handler, NULL);
/* Marshall the integer into a byte array (takes byte-order into account) */
i = 42;
printf("\nIPC_marshall(...)\n");
IPC_marshall(IPC_msgFormatter(MSG3), &i, &varcontent);
/* Publish the marshalled byte array (message handler prints the data) */
printf("\nIPC_publishVC(%s, varcontent[%d])\n", MSG3, varcontent.length);
IPC_publishVC(MSG3, &varcontent);
IPC_freeByteArray(varcontent.content);
IPC_listenClear(MSECS(1));
/* Marshall the string into a byte array. NOTE: Need to pass a *pointer*
to the string! */
string = "Hello, world";
/* It's much better (safer) to use IPC_msgFormatter, but this is included
just to illustrate the use of IPC_parseFormat */
printf("\nIPC_marshall(...)\n");
IPC_marshall(IPC_parseFormat(STRING_FORMAT), &string, &varcontent);
/* Publish the marshalled byte array (message handler prints the data) */
printf("\nIPC_publishVC(%s, varcontent[%d])\n", MSG4, varcontent.length);
IPC_publishVC(MSG4, &varcontent);
IPC_freeByteArray(varcontent.content);
IPC_listenClear(MSECS(1));
/* Set up a sample MATRIX_LIST structure */
for (k=0, i=0; i<2; i++) for (j=0; j<2; j++) m1.matrix[i][j] = (i+j+k);
m1.matrixName = "TheFirst"; m1.count = k; m1.next = &m2;
for (k++, i=0; i<2; i++) for (j=0; j<2; j++) m2.matrix[i][j] = (i+j+k);
m2.matrixName = "TheSecond"; m2.count = k; m2.next = &m3;
for (k++, i=0; i<2; i++) for (j=0; j<2; j++) m3.matrix[i][j] = (i+j+k);
m3.matrixName = "TheThird"; m3.count = k; m3.next = NULL;
/* IPC_publishData both marsalls and publishes the data structure */
printf("\nIPC_publishData(%s, m1)\n", MSG5);
IPC_publishData(MSG5, &m1);
IPC_listenClear(MSECS(1));
}
/* Use of IPC_queryResponseData and IPC_respondData --
Send out a message with a matrix_list format; The response is a
message with the first matrix, but each element incremented by one. */
{ MATRIX_LIST_TYPE m1;
MATRIX_TYPE *matrixPtr;
int i, j, k;
/* Define the "query" message */
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, %s)\n",
QUERY2_MSG, MATRIX_LIST_FORMAT);
IPC_defineMsg(QUERY2_MSG, IPC_VARIABLE_LENGTH, MATRIX_LIST_FORMAT);
/* Define the "response" message */
printf("\nIPC_defineMsg(%s, IPC_VARIABLE_LENGTH, %s)\n",
RESPONSE2_MSG, MATRIX_FORMAT);
IPC_defineMsg(RESPONSE2_MSG, IPC_VARIABLE_LENGTH, MATRIX_FORMAT);
/* Subscribe to query message with automatic unmarshalling */
printf("\nIPC_subscribeData(%s, query2Handler, NULL)\n", QUERY2_MSG);
IPC_subscribeData(QUERY2_MSG, query2Handler, NULL);
/* Set up a sample MATRIX_LIST structure */
for (k=0, i=0; i<2; i++) for (j=0; j<2; j++) m1.matrix[i][j] = (i+j+k);
m1.matrixName = "TheFirst"; m1.count = k; m1.next = NULL;
/* IPC_queryResponseData both marsalls and sends the data structure */
printf("\nIPC_queryResponseData(%s, m1, matrixPtr, IPC_WAIT_FOREVER)\n",
QUERY2_MSG);
if (IPC_queryResponseData(QUERY2_MSG, (void *)&m1,
(void **)(void *)&matrixPtr,
IPC_WAIT_FOREVER) == IPC_OK) {
IPC_printData(IPC_msgFormatter(RESPONSE2_MSG), stdout, matrixPtr);
} else {
printf("IPC_queryResponseData failed\n");
}
}
#if !defined(VXWORKS) && !defined(_WINSOCK_)
/* Don't do this for vxworks, since it does not handle stdin from the
terminal, nor for winsock, since it does not seem to be able to use
select on a non-socket fd */
/* Subscribe a handler for tty input. Now, typing at the terminal will
echo the input. Typing "q" will quit the program; typing "m" will
send a message; typing "u" will unsubscribe the handler (the program
will no longer listen to input). */
printf("\nIPC_subscribeFD(%d, stdinHnd, %s)\n", fileno(stdin), "FD1");
IPC_subscribeFD(fileno(stdin), stdinHnd, (void *)"FD1");
printf("\nEntering dispatch loop (terminal input is echoed, type 'q' to quit,\n");
printf(" 'm' to send a message, 'u' to stop listening to stdin).\n");
IPC_dispatch();
#endif
/* If ever reaches here, shut down gracefully */
IPC_disconnect();
}
void shutdown_ipc() {
IPC_disconnect();
}
/** Main loop; gets keyboard input and sends IPC messages out accordingly. */
int main(int argc, char *argv[]) {
int quit = 0;
char input;
int connectionStatus = init_ipc("darwin_nav_ipc","localhost");
if( connectionStatus == 0 ) {
// connect to IPC and register handlers
//IPC_subscribeData(DARWIN_NAV_COMMAND, navStateHandler, NULL);
// TODO: should the walk engine startup be controlled from here?
// send ourselves some messages through Central to test
Darwin_Nav_Command command;
//command.data[0] = 1.0;
//command.data[1] = 0.5;
//command.data[2] = 0.6;
//command.mode = DARWIN_NAV_STOP;
//IPC_publishData(DARWIN_NAV_COMMAND, &command);
//command.mode = DARWIN_NAV_KICK;
//IPC_publishData(DARWIN_NAV_COMMAND, &command);
//command.mode = DARWIN_NAV_SET_RELATIVE_VELOCITY;
//IPC_publishData(DARWIN_NAV_COMMAND, &command);
// print help
printf("Drive the robot with IPC!\n");
printf("i j k l (u p) or 8 4 5 6 (7 9) to steer\n");
printf("z x to kick");
printf("");
float yaw=0.0f,pitch=0.0f;
printf(" q to quit\n");
// control robot with input
while( quit==0 ) {
input = getchar();
switch(input) {
// TODO: turning should be zeroed before I try to use it like this
// TODO: have separate messages for sidestep and turn; 1 and 2 do different things
// TODO: write a left kick and a right kick message
// TODO: write a sit/stand message? also, manage walk better, a bug
// directional input // TODO: tie to frequency...
// turn left
case '4':
case 'j':
command.mode = DARWIN_NAV_SET_RELATIVE_VELOCITY;
command.data[0] = 0.0; // forwards / backwards
command.data[1] = 0.0; // sidestep left and right
command.data[2] = DARWIN_TURN_INCREMENT; // turn left and right TODO: still backward
break;
// turn right
case '6':
case 'l':
command.mode = DARWIN_NAV_SET_RELATIVE_VELOCITY;
command.data[0] = 0.0;
command.data[1] = 0.0;
command.data[2] = -DARWIN_TURN_INCREMENT;
break;
// sidestep left
case '7':
case 'u':
command.mode = DARWIN_NAV_SET_RELATIVE_VELOCITY;
command.data[0] = 0.0;
command.data[1] = DARWIN_TURN_INCREMENT;
command.data[2] = 0.0;
break;
// sidestep right
case '9':
case 'o':
command.mode = DARWIN_NAV_SET_RELATIVE_VELOCITY;
command.data[0] = 0.0;
command.data[1] = -DARWIN_TURN_INCREMENT;
command.data[2] = 0.0;
break;
// go forward
case '8':
case 'i':
command.mode = DARWIN_NAV_SET_RELATIVE_VELOCITY;
command.data[0] = DARWIN_WALK_INCREMENT;
command.data[1] = 0.0;
command.data[2] = 0.0;
break;
// kick left foot
case 'z':
command.mode = DARWIN_NAV_KICK_LEFT;
break;
// kick right foot
case 'x':
command.mode = DARWIN_NAV_KICK_RIGHT;
break;
// stop TODO: slow down
case '5':
case 'k':
command.mode = DARWIN_NAV_SET_ABSOLUTE_VELOCITY; //TODO: may not work? fix
command.data[0] = 0.0;
command.data[1] = 0.0;
command.data[2] = 0.0;
//command.mode = DARWIN_NAV_STOP;
break;
case '1':
command.mode = DARWIN_NAV_STOP;
break;
case '2':
command.mode = DARWIN_NAV_SIT;
break;
case '3':
command.mode = DARWIN_NAV_START;
break;
case 'q':
printf("Quitting...\n");
command.mode = DARWIN_NAV_QUIT;
quit=1;
break;
case 'r':
//new head turn entry added 6/15;
command.mode=DARWIN_NAV_SET_RELATIVE_HEAD;
command.data[0]=(1)*(PI/180);
yaw+=command.data[0];
break;
case 't':
command.mode=DARWIN_NAV_SET_RELATIVE_HEAD;
command.data[0]=-(1)*(PI/180);
yaw+=command.data[0];
break;
case 'f':
command.mode=DARWIN_NAV_SET_RELATIVE_HEAD;
command.data[0]=0.0;
command.data[1]=((1)*(PI/180));
pitch+=command.data[1];
break;
case 'g':
command.mode=DARWIN_NAV_SET_RELATIVE_HEAD;
command.data[0]=0.0;
command.data[1]=-(1)*(PI/180);
pitch+=command.data[1];
break;
}
IPC_publishData(DARWIN_NAV_COMMAND, &command);
printf("yaw=%f,pitch=%f\n",yaw,pitch);
}// end while loop
}
else {
printf("IPC connect failed, quitting...\n");
return 0;
}
IPC_disconnect();
}
int main(int argc, char *argv[]){
GCM_Common_RobotState curState;
bool quit=false;
char *pointer;
GCM_Common_Command *gcc = (GCM_Common_Command *)malloc(sizeof(GCM_Common_Command));
GCM_logSetFilename((char *)"snmd-control.log");
initIPC((char *)"SNMD::Control", argv[1], &curState);
printf("SNMD command interface. Press '?' for more information.\n");
strncpy(gcc->name,"nav",64);
while(!quit){
IPC_listenClear(100);
pointer=readline("> ");
switch(pointer[0]){
case '?':
printf("Command functions:\n");
printf("==================\n");
printf("?: This screen\n");
printf("a: Issue an achieve command with (x,y) coordinates\n");
printf("b: Wall following backward\n");
printf("c: Check SNMD status\n");
printf("f: Wall following forward\n");
printf("g: Issue an achieve command with (x,y,theta) coordinates \n");
printf("h: Halt the robot (idle)\n");
printf("m: Issue a move command\n");
printf("o: Issue an orient command\n");
printf("r: Wander\n");
printf("s: Soft stop\n");
printf("t: Run the testbed\n");
printf("w: Use waypoint following\n");
printf("x: Exit snmd-control but don't halt robot\n");
printf("q: Quit snmd-control and halt SNMD\n");
break;
case 'c':
printf("Position: %.2f %.2f %.2f\n", curState.x, curState.y, curState.t );
break;
case 'p':
// tell the robot to achieve
pointer = readline("% X: ");
gcc->data[0] = atof(pointer);
pointer = readline("% Y: ");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0; // punt on the angle here
printf("Sending achieve point %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "absolute");
gcc->mode = GCM_NAV_ACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'a':
// tell the robot to achieve a relative location
pointer = readline("% dX(m): ");
gcc->data[0] = atof(pointer);
pointer = readline("% dY(m): ");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0; // punt on the angle here
printf("Sending relative achieve %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "relative");
gcc->mode = GCM_NAV_ACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'd':
// tell the robot to approach (forward or backward)
pointer = readline("% dX(m): ");
gcc->data[0] = atof(pointer);
pointer = readline("% dApp(m): ");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0; // no need for an angle here
printf("Sending approach %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "approach");
gcc->mode = GCM_NAV_ACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'g':
// tell the robot to go to a relative location
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "sn relative");
gcc->mode = GCM_NAV_ACHIEVE;
pointer = readline("% dX(m): ");
// enter "verbose" instead of an x value to short-circuit the command
// and instead toggle verbose mode for the achieve_sn competency
if (!strncmp(pointer, "verbose", 16)) {
strcpy(gcc->cdata, "toggle verbose");
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
}
gcc->data[0] = atof(pointer);
pointer = readline("% dY(m): ");
gcc->data[1] = atof(pointer);
pointer = readline("% dT(m): ");
gcc->data[2] = atof(pointer);
printf("Sending relative achieve %.2f %.2f %.2f\n",
gcc->data[0], gcc->data[1], gcc->data[2]);
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'm':
// tell the robot to move
pointer=readline("% Trans Vel (m/s)? ");
gcc->data[0]=atof(pointer);
printf("Data: %f\n",gcc->data[0]);
pointer=readline("% Rot Vel (rad/s)? ");
gcc->data[1]=atof(pointer);
printf("Data: %f\n",gcc->data[1]);
gcc->mode=GCM_NAV_MOVE;
IPC_publishData(GCM_COMMON_COMMAND,gcc);
break;
case 't':
gcc->mode = GCM_PLAN_PATH;
strncpy(gcc->name, "planner", 64);
pointer = readline("% X (meters): ");
gcc->data[0] = atof(pointer);
pointer = readline("% Y (meters): ");
gcc->data[1] = atof(pointer);
IPC_publishData(GCM_COMMON_COMMAND, gcc);
strncpy(gcc->name, "nav", 64);
break;
/*
printf("snmd-control.c: Entering the testbed case.\n");
// tell the robot to move
pointer=readline("% Max Trans? ");
gcc->data[0]=atof(pointer);
printf("Data: %f\n",gcc->data[0]);
pointer=readline("% Max Rotation? ");
gcc->data[1]=atof(pointer);
printf("Data: %f\n",gcc->data[1]);
gcc->mode=GCM_NAV_TEST;
IPC_publishData(GCM_COMMON_COMMAND,gcc);
break;
*/
case 'o':
// tell the robot to orient
pointer=readline("Angle (rad) ");
gcc->data[0]=atof(pointer);
printf("Data: %f\n",gcc->data[0]);
gcc->mode=GCM_NAV_ANGACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND,gcc);
break;
case 'w':
{
int i;
pointer = readline(" % Waypoints:");
int numWaypoints = atoi(pointer);
printf("Calculation method is (a)rcs and circles, (s)plines, sta(n)dard?:\n");
pointer = readline(">> ");
strncpy( gcc->cdata, pointer, 64 );
for (i=0; i < numWaypoints; i++)
{
pointer = readline ("% X:");
gcc->data[0] = atof(pointer);
pointer = readline("% Y:");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0;
gcc->data[3] = i; // stores the current waypoint number
printf("Adding point to the queue %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
gcc->mode = GCM_NAV_WAYPOINT;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
}
gcc->mode = GCM_NAV_EXECWP;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
}
case 'r':
// tell nav to wander
printf("Sending wander...\n");
gcc->mode=GCM_NAV_AUTO;
strcpy(gcc->cdata, "wander");
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
case 's': // soft stop
// stop planner module if it was running
strcpy(gcc->name, "planner");
gcc->mode = GCM_COMMON_IDLE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
// coast robot to stop
printf("Soft stop...\n");
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "soft stop");
gcc->mode = GCM_NAV_AUTO;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'h': // hard stop (halt)
// stop planner module if it was running
strcpy(gcc->name, "planner");
gcc->mode = GCM_COMMON_IDLE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
// tell nav to idle
printf("Halt! Sending idle...\n");
strcpy(gcc->name, "nav");
gcc->mode=GCM_COMMON_IDLE;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
case 'x':
quit = true;
break;
case 'q':
printf("G'bye.\n");
gcc->mode=GCM_COMMON_QUIT;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
quit=true;
break;
case 'f':
// tell nav to wall follow forward
printf("Sending wall follow...\n");
gcc->mode=GCM_NAV_AUTO;
strcpy(gcc->cdata, "wallFollow");
gcc->data[0] = WALLFOLLOW_FORWARD;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
case 'b':
// tell nav to wall follow backward
printf("Sending wall follow...\n");
gcc->mode=GCM_NAV_AUTO;
strcpy(gcc->cdata, "wallFollow");
gcc->data[0] = WALLFOLLOW_BACKWARD;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
default:
printf("Unknown command %c\n", pointer[0]);
break;
}
free(pointer);
}
IPC_disconnect();
printf("Goodbye...\n");
return 0;
}
