///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); }
/* 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); }
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 }
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) command_tv = carmen_clamp(carmen_robot_laser_min_rear_velocity(), command_tv,
carmen_warn("Loading parameters for robot [31;1m%s[0m using param file " "[31;1m%s[0m\n", selected_robot, param_filename); return 0; } static int lookup_ipc_query(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData __attribute__ ((unused)), carmen_param_query_message *query) { FORMATTER_PTR formatter; IPC_RETURN_TYPE err = IPC_OK; formatter = IPC_msgInstanceFormatter(msgRef); err = IPC_unmarshallData(formatter, callData, query, sizeof(carmen_param_query_message)); IPC_freeByteArray(callData); carmen_test_ipc_return_int(err, "Could not unmarshall", IPC_msgInstanceName(msgRef)); return lookup_parameter(query->module_name, query->variable_name); } static void get_robot(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData __attribute__ ((unused))) { FORMATTER_PTR formatter; IPC_RETURN_TYPE err = IPC_OK; carmen_param_query_message query;
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; }
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 dot_query_handler (MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData __attribute__ ((unused))) { FORMATTER_PTR formatter; IPC_RETURN_TYPE err; carmen_dot_query query; formatter = IPC_msgInstanceFormatter(msgRef); err = IPC_unmarshallData(formatter, callData, &query, sizeof(carmen_dot_query)); IPC_freeByteArray(callData); carmen_test_ipc_return(err, "Could not unmarshall", IPC_msgInstanceName(msgRef)); printf("--> received query %d\n", query.type); switch(query.type) { case CARMEN_DOT_PERSON: respond_all_people_msg(msgRef); break; case CARMEN_DOT_TRASH: respond_all_trash_msg(msgRef); break; case CARMEN_DOT_DOOR: respond_all_doors_msg(msgRef);