///handler needed for odom message and velocity message void qc_imu_imu_message_handler(MSG_INSTANCE mi, BYTE_ARRAY callData, void* clientData){ (void) clientData; //no warning IPC_RETURN_TYPE err = IPC_OK; err = IPC_unmarshallData(IPC_msgInstanceFormatter(mi), callData, &imsg, sizeof(imsg)); newImuMessageAvailable = true; IPC_freeByteArray(callData); IPC_freeDataElements(IPC_msgInstanceFormatter(mi),&imsg); }
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); }
/* Message handler function - It gets called when we receive a message * with the name "ListeningToAndroid" */ static void msgHandler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData) { drive_arc_command_msg_type msg1; double radius, time, speed; char sender[20]; time_type timestamp; IPC_unmarshallData(IPC_msgInstanceFormatter(msgRef), callData, &msg1, sizeof(msg1)); fp=fopen("CommandLog.txt","a"); fprintf(fp,"Command Radius: %f, Speed: %f, Time: %f",msg1.radius,msg1.speed,msg1.time); fprintf(fp,"\tSender: %s",msg1.sender.data); time_t commandTime=msg1.timestamp.tv_sec; fprintf(fp,"\tTimestamp: %s", ctime(&commandTime)); fclose(fp); sendZoeResponse(msg1); IPC_freeByteArray(callData); }
/* 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 msgHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData) { FORMATTER_PTR formatter; carmen_test_ipc_message msg; formatter = IPC_msgInstanceFormatter(msgRef); IPC_unmarshallData(formatter, callData, &msg, sizeof(carmen_test_ipc_message)); IPC_freeByteArray(callData); #if (defined(__x86_64__)) fprintf(stderr, "Got message from client %ld : time %f\n", (long)clientData, msg.timestamp); #else fprintf(stderr, "Got message from client %d : time %f\n", (int)clientData, msg.timestamp); #endif }
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); }
msg.vector_distance = distance; msg.vector_angle = angle; err = IPC_publishData(CARMEN_ROBOT_VECTOR_STATUS_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VECTOR_STATUS_NAME); } 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)
JNIEXPORT jlong JNICALL Java_ipc_java_IPC_IPC_1msgInstanceFormatter (JNIEnv *env, jclass theClass, jlong msgInstance) { return (jlong)(size_t)IPC_msgInstanceFormatter((MSG_INSTANCE)(size_t)msgInstance); }
void qc_odometry_odometry_message_handler(MSG_INSTANCE mi, BYTE_ARRAY callData, void* clientData){ (void) clientData; //no warning IPC_RETURN_TYPE err = IPC_OK; prev_omsg = omsg; err = IPC_unmarshallData(IPC_msgInstanceFormatter(mi), callData, &omsg, sizeof(omsg)); if (newImuMessageAvailable){ newImuMessageAvailable = false; prev_vmsg = vmsg; vmsg.timestamp_sec = omsg.timestamp_sec; vmsg.timestamp_usec = omsg.timestamp_usec; if (msgcounter > 2) validData = true; if (validData){ double dtv = omsg.timestamp_sec - prev_omsg.timestamp_sec + 1e-6 * (omsg.timestamp_usec - prev_omsg.timestamp_usec); double dta = vmsg.timestamp_sec - prev_vmsg.timestamp_sec + 1e-6 * (vmsg.timestamp_usec - prev_vmsg.timestamp_usec); vmsg.x = omsg.x; vmsg.y = omsg.y; vmsg.z = omsg.z; vmsg.q0 = omsg.q0; vmsg.q1 = omsg.q1; vmsg.q2 = omsg.q2; vmsg.q3 = omsg.q3; vmsg.roll = omsg.roll; vmsg.pitch = omsg.pitch; vmsg.yaw = omsg.yaw; Transformation3<double> delta; Transformation3<double> p0(DVector3(prev_omsg.x, prev_omsg.y, prev_omsg.z), Quaternion<double>(prev_omsg.q0, prev_omsg.q1, prev_omsg.q2, prev_omsg.q3)); Transformation3<double> p1(DVector3(omsg.x, omsg.y, omsg.z), Quaternion<double>(omsg.q0, omsg.q1, omsg.q2, omsg.q3)); delta = p0.inv() * p1; ///velocity && acceleration based on omsg if (dtv > 1e-7){ dtv = 1./dtv; vmsg.vx = delta.translationVector.x() * dtv; vmsg.vy = delta.translationVector.y() * dtv; vmsg.vz = delta.translationVector.z() * dtv; if (dta > 1e-7){ ///take into account the different orientations of the velocities! p0.translationVector = DVector3(0,0,0); p0.rotationQuaternion = Quaternion<double>(prev_vmsg.q0, prev_vmsg.q1, prev_vmsg.q2, prev_vmsg.q3); p0.translationVector = p0 * DVector3(prev_vmsg.vx, prev_vmsg.vy, prev_vmsg.vz); p1.translationVector = DVector3(0,0,0); p1.rotationQuaternion = Quaternion<double>(vmsg.q0, vmsg.q1, vmsg.q2, vmsg.q3); p1.translationVector = p1 * DVector3(vmsg.vx, vmsg.vy, vmsg.vz); delta = p0.inv() * p1; dta = 1./dta; vmsg.avx = delta.translationVector.x() * dta; vmsg.avy = delta.translationVector.y() * dta; vmsg.avz = delta.translationVector.z() * dta; } else { vmsg.avx = vmsg.avy = vmsg.avz = 0; } } else { vmsg.avx = vmsg.avy = vmsg.avz = 0; vmsg.vx = vmsg.vy = vmsg.vz = 0; } ///acceleration based on imu data ///FIXME ///acceleration based on tan roll and tan pitch Quaternion<double> q(imsg.q0, imsg.q1, imsg.q2, imsg.q3); DVector3 an = q.toAngles(); vmsg.axtanp = tan(an.pitch()) * 9.81; vmsg.aytanr = tan(an.roll()) * 9.81; qc_odometry_publish_velocity_message(&vmsg); } } IPC_freeByteArray(callData); IPC_freeDataElements(IPC_msgInstanceFormatter(mi),&omsg); msgcounter++; if (!(msgcounter%10)) std::cerr << "." << std::flush; }