static void respond_all_doors_msg(MSG_INSTANCE msgRef) {
static carmen_dot_all_doors_msg msg;
static int first = 1;
IPC_RETURN_TYPE err;
int i, n;
if (first) {
first = 0;
msg.doors = NULL;
strcpy(msg.host, carmen_get_tenchar_host_name());
}
for (n = i = 0; i < num_filters; i++)
if (filters[i].type == CARMEN_DOT_DOOR)
n++;
msg.num_doors = n;
if (n == 0)
msg.doors = NULL;
else {
msg.doors = (carmen_dot_door_p)
realloc(msg.doors, n*sizeof(carmen_dot_door_t));
carmen_test_alloc(msg.doors);
for (n = i = 0; i < num_filters; i++) {
if (filters[i].type == CARMEN_DOT_DOOR) {
msg.doors[n].id = filters[i].id;
msg.doors[n].x = filters[i].door_filter.x;
msg.doors[n].y = filters[i].door_filter.y;
msg.doors[n].theta = filters[i].door_filter.t;
msg.doors[n].vx = filters[i].door_filter.px;
msg.doors[n].vy = filters[i].door_filter.py;
msg.doors[n].vxy = filters[i].door_filter.pxy;
msg.doors[n].vtheta = filters[i].door_filter.pt;
n++;
}
}
}
msg.timestamp = carmen_get_time_ms();
err = IPC_respondData(msgRef, CARMEN_DOT_ALL_DOORS_MSG_NAME, &msg);
carmen_test_ipc(err, "Could not respond",
CARMEN_DOT_ALL_DOORS_MSG_NAME);
}
static void respond_all_people_msg(MSG_INSTANCE msgRef) {
static carmen_dot_all_people_msg msg;
static int first = 1;
IPC_RETURN_TYPE err;
int i, n;
if (first) {
first = 0;
msg.people = NULL;
strcpy(msg.host, carmen_get_tenchar_host_name());
}
for (n = i = 0; i < num_filters; i++)
if (filters[i].type == CARMEN_DOT_PERSON)
n++;
msg.num_people = n;
if (n == 0)
msg.people = NULL;
else {
msg.people = (carmen_dot_person_p)
realloc(msg.people, n*sizeof(carmen_dot_person_t));
carmen_test_alloc(msg.people);
for (n = i = 0; i < num_filters; i++) {
if (filters[i].type == CARMEN_DOT_PERSON) {
msg.people[n].id = filters[i].id;
msg.people[n].x = filters[i].person_filter.x;
msg.people[n].y = filters[i].person_filter.y;
msg.people[n].vx = filters[i].person_filter.px;
msg.people[n].vy = filters[i].person_filter.py;
msg.people[n].vxy = filters[i].person_filter.pxy;
n++;
}
}
}
msg.timestamp = carmen_get_time_ms();
err = IPC_respondData(msgRef, CARMEN_DOT_ALL_PEOPLE_MSG_NAME, &msg);
carmen_test_ipc(err, "Could not respond",
CARMEN_DOT_ALL_PEOPLE_MSG_NAME);
}
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 get_arm_state()
{
carmen_arm_direct_get_state(arm_state.joint_angles, arm_state.joint_currents, arm_state.joint_angular_vels, NULL);
arm_state.timestamp = carmen_get_time();
}
static void arm_query_handler(MSG_INSTANCE msgRef,
BYTE_ARRAY callData __attribute__ ((unused)),
void *clientData __attribute__ ((unused)))
{
IPC_RETURN_TYPE err;
get_arm_state();
err = IPC_respondData(msgRef, CARMEN_ARM_STATE_NAME, &arm_state);
}
static void arm_command_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused)))
{
IPC_RETURN_TYPE err;
carmen_arm_command_message msg;
FORMATTER_PTR formatter;
formatter = IPC_msgInstanceFormatter(msgRef);
err = IPC_unmarshallData(formatter, callData, &msg,
sizeof(carmen_arm_command_message));
IPC_freeByteArray(callData);
if (msg.num_joints != arm_model.num_joints)
