static void subject_publish_callback(const ViconDriver::Subject &subject) { static std::set<std::string> defined_msgs; std::string msgname = "vicon_" + subject.name; if(defined_msgs.find(msgname) == defined_msgs.end()) { if(pthread_mutex_trylock(&ipc_mutex) == 0) { IPC_defineMsg(msgname.c_str(), IPC_VARIABLE_LENGTH, ViconSubject::getIPCFormat()); pthread_mutex_unlock(&ipc_mutex); defined_msgs.insert(msgname); } else return; } if(loadCalib(subject.name)) { Eigen::Affine3d current_pose = Eigen::Affine3d::Identity(); current_pose.translate(Eigen::Vector3d(subject.translation)); current_pose.rotate(Eigen::Quaterniond(subject.rotation)); current_pose = current_pose*calib_pose[subject.name]; const Eigen::Vector3d position(current_pose.translation()); const Eigen::Quaterniond rotation(current_pose.rotation()); ViconSubject subject_ipc; subject_ipc.time_sec = subject.time_usec/1000000; // Integer division subject_ipc.time_usec = subject.time_usec - subject_ipc.time_sec*1000000; subject_ipc.frame_number = subject.frame_number; subject_ipc.name = const_cast<char*>(subject.name.c_str()); subject_ipc.occluded = subject.occluded; subject_ipc.position[0] = position.x(); subject_ipc.position[1] = position.y(); subject_ipc.position[2] = position.z(); subject_ipc.orientation[0] = rotation.x(); subject_ipc.orientation[1] = rotation.y(); subject_ipc.orientation[2] = rotation.z(); subject_ipc.orientation[3] = rotation.w(); subject_ipc.num_markers = subject.markers.size(); subject_ipc.markers = new ViconMarker[subject_ipc.num_markers]; for(int i = 0; i < subject_ipc.num_markers; i++) { subject_ipc.markers[i].name = const_cast<char*>(subject.markers[i].name.c_str()); subject_ipc.markers[i].subject_name = const_cast<char*>(subject.markers[i].subject_name.c_str()); subject_ipc.markers[i].position[0] = subject.markers[i].translation[0]; subject_ipc.markers[i].position[1] = subject.markers[i].translation[1]; subject_ipc.markers[i].position[2] = subject.markers[i].translation[2]; subject_ipc.markers[i].occluded = subject.markers[i].occluded; } if(pthread_mutex_trylock(&ipc_mutex) == 0) { IPC_publishData(msgname.c_str(), &subject_ipc); pthread_mutex_unlock(&ipc_mutex); } delete[] subject_ipc.markers; } }
boolean IMU::init(int aPinBuzzer){ pinBuzzer = aPinBuzzer; loadCalib(); printCalib(); if (!initL3G4200D()) return false; initADXL345B(); initHMC5883L(); now = 0; hardwareInitialized = true; return true; }