/* Handles the response to QUERY_MSG messages */
static void replyHandler (MSG_INSTANCE msgRef,
BYTE_ARRAY callData, void *clientData)
{
printf("replyHandler: Receiving message %s (%s) [%s]\n",
IPC_msgInstanceName(msgRef), (char *)callData, (char *)clientData);
IPC_freeByteArray(callData);
}
JNIEXPORT jstring JNICALL
Java_ipc_java_IPC_IPC_1msgInstanceName (JNIEnv *env, jclass theClass,
jlong msgInstance)
{
const char *cmsgName;
cmsgName = IPC_msgInstanceName((MSG_INSTANCE)(size_t)msgInstance);
return (*env)->NewStringUTF(env, cmsgName);
}
static void queryReplyHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData)
{
QUERY_REPLY_PTR queryReplyData = (QUERY_REPLY_PTR)clientData;
queryReplyData->handled = TRUE;
queryReplyData->data = callData;
queryReplyData->formatter = IPC_msgInstanceFormatter(msgRef);
queryReplyData->msgName = IPC_msgInstanceName(msgRef);
}
static void ipcDisconnectHandler (MSG_INSTANCE ref, char **moduleNamePtr)
{
CONNECT_DATA_PTR disconnectData;
LIST_PTR list;
LOCK_CM_MUTEX;
list = GET_C_GLOBAL(disconnectNotifyList);
UNLOCK_CM_MUTEX;
for (disconnectData = (CONNECT_DATA_PTR)x_ipc_listFirst(list); disconnectData;
disconnectData = (CONNECT_DATA_PTR)x_ipc_listNext(list)) {
(disconnectData->handler)(*moduleNamePtr, disconnectData->clientData);
}
x_ipcFreeData(IPC_msgInstanceName(ref), moduleNamePtr); /* ??? */
}
/* Unmarshall the byte array and print it out. */
static void msg3Handler (MSG_INSTANCE msgRef,
BYTE_ARRAY callData, void *clientData)
{
#ifdef UNUSED_PRAGMA
#pragma unused(clientData)
#endif
FORMATTER_PTR formatter;
void *dataptr;
printf("msg3Handler: Receiving message %s: ", IPC_msgInstanceName(msgRef));
/* Get the formatter for this message instance */
formatter = IPC_msgInstanceFormatter(msgRef);
IPC_unmarshall(formatter, callData, &dataptr);
IPC_printData(formatter, stdout, dataptr);
/* Free up malloc'd data */
IPC_freeByteArray(callData);
IPC_freeData(formatter, dataptr);
}
static void queryHandler (MSG_INSTANCE msgRef,
BYTE_ARRAY callData, void *clientData)
{
char *publishStr = "Published";
char *responseStr = "Responded";
printf("queryHandler: Receiving message %s of %d bytes (%s) [%s]\n",
IPC_msgInstanceName(msgRef), IPC_dataLength(msgRef),
(char *)callData, (char *)clientData);
/* Publish this message -- all subscribers get it */
printf("\n IPC_publish(%s, %d, %s)\n",
MSG2, (int)strlen(publishStr)+1, publishStr);
IPC_publish(MSG2, strlen(publishStr)+1, publishStr);
/* Respond with this message -- only the query handler gets it */
printf("\n IPC_respond(msgRef, %s, %d, %s)\n",
MSG2, (int)strlen(responseStr)+1, responseStr);
IPC_respond(msgRef, MSG2, strlen(responseStr)+1, responseStr);
IPC_freeByteArray(callData);
}
static void query2Handler (MSG_INSTANCE msgRef,
void *callData, void *clientData)
{
MATRIX_LIST_TYPE *matrixList = (MATRIX_LIST_TYPE *)callData;
FORMATTER_PTR formatter;
int i, j;
printf("query2Handler: Receiving message %s [%s]\n",
IPC_msgInstanceName(msgRef),
(clientData ? (char *)clientData : "NULL"));
formatter = IPC_msgInstanceFormatter(msgRef);
IPC_printData(formatter, stdout, matrixList);
for (i=0; i<2; i++) for (j=0; j<2; j++) matrixList->matrix[i][j] += 1;
printf("\n IPC_respondData(msgRef, %s, matrixList->matrix)\n",
RESPONSE2_MSG);
IPC_respondData(msgRef, RESPONSE2_MSG, &(matrixList->matrix));
/* Free up malloc'd data */
IPC_freeData(formatter, matrixList);
}
static void
velocity_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused)))
{
carmen_robot_velocity_message v;
FORMATTER_PTR formatter;
IPC_RETURN_TYPE err;
formatter = IPC_msgInstanceFormatter(msgRef);
err = IPC_unmarshallData(formatter, callData, &v,
sizeof(carmen_robot_velocity_message));
IPC_freeByteArray(callData);
carmen_test_ipc_return(err, "Could not unmarshall",
IPC_msgInstanceName(msgRef));
time_of_last_command = carmen_get_time_ms();
command_rv = v.rv;
command_tv = v.tv;
if (collision_avoidance)
{
if (use_laser)
command_tv = carmen_clamp(carmen_robot_laser_min_rear_velocity(),
command_tv,
carmen_robot_laser_max_front_velocity());
if (use_sonar)
command_tv = carmen_clamp(carmen_robot_sonar_min_rear_velocity(),
command_tv,
