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;
}
