void QualisysDriver::handleFrame() {
// Number of rigid bodies
int body_count = prt_packet->Get6DOFEulerBodyCount();
// Assign each subject with a thread
map<string, boost::shared_ptr<boost::thread> > subject_threads;
for (int i = 0; i< body_count; ++i) {
string subject_name(
port_protocol.Get6DOFBodyName(i));
// Process the subject if required
if (model_set.empty() || model_set.count(subject_name)) {
// Create a new subject if it does not exist
if (subjects.find(subject_name) == subjects.end()) {
subjects[subject_name] = Subject::SubjectPtr(
new Subject(&nh, subject_name, fixed_frame_id));
subjects[subject_name]->setParameters(
process_noise, measurement_noise, frame_rate);
}
// Handle the subject in a different thread
subject_threads[subject_name] =
boost::shared_ptr<boost::thread>(
new boost::thread(&QualisysDriver::handleSubject, this, i));
//handleSubject(i);
}
}
// Wait for all the threads to stop
for (auto it = subject_threads.begin();
it != subject_threads.end(); ++it) {
it->second->join();
}
// Send out warnings
for (auto it = subjects.begin();
it != subjects.end(); ++it) {
Subject::Status status = it->second->getStatus();
if (status == Subject::LOST)
ROS_WARN_THROTTLE(1, "Lose track of subject %s", (it->first).c_str());
else if (status == Subject::INITIALIZING)
ROS_WARN("Initialize subject %s", (it->first).c_str());
}
return;
}
// ////////////////////////////////////////////////////////////////////////////
void SubjectNameFlat::CheckReplyName()
{
try {
CheckFullName();
if (!IsReplyName()) {
// CODE NOTE: Should throw RvBadSubjectException
std::string subject_name(ToString());
throw RvException("The length of subject name '" + subject_name +
"' (" + MLB::Utility::AnyToString(subject_name.size()) +
") exceeds the maximum permissible (" +
MLB::Utility::AnyToString(MaxReplySubjectSize) + ").");
}
}
catch (const std::exception &except) {
MLB::Utility::Rethrow(except, "Not a valid Tib/Rendezvous reply "
"subject: " + std::string(except.what()));
}
}
void QualisysDriver::handleSubject(const int& sub_idx) {
boost::unique_lock<boost::shared_mutex> write_lock(mtx);
// Name of the subject
string subject_name(port_protocol.Get6DOFBodyName(sub_idx));
// Pose of the subject
float x, y, z, roll, pitch, yaw;
prt_packet->Get6DOFEulerBody(
sub_idx, x, y, z, roll, pitch, yaw);
write_lock.unlock();
// If the subject is lost
if(isnan(x) || isnan(y) || isnan(z) ||
isnan(roll) || isnan(pitch) || isnan(yaw)) {
subjects[subject_name]->disable();
return;
}
// Qualisys sometimes flips 180 degrees around the x axis
//if(roll > 90)
// roll -= 180;
//else if(roll < -90)
// roll += 180;
// Convert the msgs to Eigen type
Eigen::Quaterniond m_att;
tf::quaternionTFToEigen(
tf::createQuaternionFromRPY(roll*deg2rad, pitch*deg2rad, yaw*deg2rad), m_att);
// Convert mm to m
Eigen::Vector3d m_pos(x/1000.0, y/1000.0, z/1000.0);
// Re-enable the object if it is lost previously
if (subjects[subject_name]->getStatus() == Subject::LOST) {
subjects[subject_name]->enable();
}
// Compute the timestamp
// double time = ros::Time::now().toSec();
if(start_time_local_ == 0)
{
start_time_local_ = ros::Time::now().toSec();
start_time_packet_ = prt_packet->GetTimeStamp() / 1e6;
}
const double packet_time = prt_packet->GetTimeStamp() / 1e6;
const double time = start_time_local_ + (packet_time - start_time_packet_);
// Feed the new measurement to the subject
subjects[subject_name]->processNewMeasurement(time, m_att, m_pos);
// Publish tf if requred
if (publish_tf &&
subjects[subject_name]->getStatus() == Subject::TRACKED) {
Quaterniond att = subjects[subject_name]->getAttitude();
Vector3d pos = subjects[subject_name]->getPosition();
tf::Quaternion att_tf;
tf::Vector3 pos_tf;
tf::quaternionEigenToTF(att, att_tf);
tf::vectorEigenToTF(pos, pos_tf);
tf::StampedTransform stamped_transform =
tf::StampedTransform(tf::Transform(att_tf, pos_tf),
ros::Time::now(), fixed_frame_id, subject_name);
write_lock.lock();
tf_publisher.sendTransform(stamped_transform);
write_lock.unlock();
}
return;
}
void QualisysDriver::handlePacketData(CRTPacket* prt_packet) {
// Number of rigid bodies
int body_count = prt_packet->Get6DOFEulerBodyCount();
// Check the publishers for the rigid bodies
checkPublishers(body_count);
// Publish data for each rigid body
for(int i = 0; i < body_count; ++i) {
float x, y, z, roll, pitch, yaw;
prt_packet->Get6DOFEulerBody(i, x, y, z, roll, pitch, yaw);
if(isnan(x) || isnan(y) || isnan(z) ||
isnan(roll) || isnan(pitch) || isnan(yaw)) {
ROS_WARN_STREAM_THROTTLE(3, "Rigid-body " << i + 1 << "/"
<< body_count << " not detected");
continue;
}
// ROTATION: GLOBAL (FIXED) X Y Z (R P Y)
//std::stringstream name;
//name << port_protocol.Get6DOFBodyName(i);
string subject_name(port_protocol.Get6DOFBodyName(i));
// Qualisys sometimes flips 180 degrees around the x axis
if(roll > 90)
roll -= 180;
else if(roll < -90)
roll += 180;
// Send transform
tf::StampedTransform stamped_transform = tf::StampedTransform(
tf::Transform(
tf::createQuaternionFromRPY(
roll * deg2rad, pitch * deg2rad, yaw * deg2rad),
tf::Vector3(x, y, z) / 1000.),
ros::Time::now(), "qualisys", subject_name);
if (publish_tf)
tf_publisher.sendTransform(stamped_transform);
// Send Subject msg
geometry_msgs::TransformStamped geom_stamped_transform;
tf::transformStampedTFToMsg(stamped_transform,
geom_stamped_transform);
qualisys::Subject subject_msg;
subject_msg.header =
geom_stamped_transform.header;
subject_msg.name = subject_name;
subject_msg.position.x =
geom_stamped_transform.transform.translation.x;
subject_msg.position.y =
geom_stamped_transform.transform.translation.y;
subject_msg.position.z =
geom_stamped_transform.transform.translation.z;
subject_msg.orientation =
geom_stamped_transform.transform.rotation;
subject_publishers[subject_name].publish(subject_msg);
}
return;
}
RefPtr<DtlsIdentity> DtlsIdentity::Generate() {
UniquePK11SlotInfo slot(PK11_GetInternalSlot());
if (!slot) {
return nullptr;
}
uint8_t random_name[16];
SECStatus rv = PK11_GenerateRandomOnSlot(slot.get(), random_name,
sizeof(random_name));
if (rv != SECSuccess)
return nullptr;
std::string name;
char chunk[3];
for (size_t i = 0; i < sizeof(random_name); ++i) {
SprintfLiteral(chunk, "%.2x", random_name[i]);
name += chunk;
}
std::string subject_name_string = "CN=" + name;
UniqueCERTName subject_name(CERT_AsciiToName(subject_name_string.c_str()));
if (!subject_name) {
return nullptr;
}
unsigned char paramBuf[12]; // OIDs are small
SECItem ecdsaParams = { siBuffer, paramBuf, sizeof(paramBuf) };
SECOidData* oidData = SECOID_FindOIDByTag(SEC_OID_SECG_EC_SECP256R1);
if (!oidData || (oidData->oid.len > (sizeof(paramBuf) - 2))) {
return nullptr;
}
ecdsaParams.data[0] = SEC_ASN1_OBJECT_ID;
ecdsaParams.data[1] = oidData->oid.len;
memcpy(ecdsaParams.data + 2, oidData->oid.data, oidData->oid.len);
ecdsaParams.len = oidData->oid.len + 2;
SECKEYPublicKey *pubkey;
UniqueSECKEYPrivateKey private_key(
PK11_GenerateKeyPair(slot.get(),
CKM_EC_KEY_PAIR_GEN, &ecdsaParams, &pubkey,
PR_FALSE, PR_TRUE, nullptr));
if (private_key == nullptr)
return nullptr;
UniqueSECKEYPublicKey public_key(pubkey);
pubkey = nullptr;
UniqueCERTSubjectPublicKeyInfo spki(
SECKEY_CreateSubjectPublicKeyInfo(public_key.get()));
if (!spki) {
return nullptr;
}
UniqueCERTCertificateRequest certreq(
CERT_CreateCertificateRequest(subject_name.get(), spki.get(), nullptr));
if (!certreq) {
return nullptr;
}
// From 1 day before todayto 30 days after.
// This is a sort of arbitrary range designed to be valid
// now with some slack in case the other side expects
// some before expiry.
//
// Note: explicit casts necessary to avoid
// warning C4307: '*' : integral constant overflow
static const PRTime oneDay = PRTime(PR_USEC_PER_SEC)
* PRTime(60) // sec
* PRTime(60) // min
* PRTime(24); // hours
PRTime now = PR_Now();
PRTime notBefore = now - oneDay;
PRTime notAfter = now + (PRTime(30) * oneDay);
UniqueCERTValidity validity(CERT_CreateValidity(notBefore, notAfter));
if (!validity) {
return nullptr;
}
unsigned long serial;
// Note: This serial in principle could collide, but it's unlikely
rv = PK11_GenerateRandomOnSlot(slot.get(),
reinterpret_cast<unsigned char *>(&serial),
sizeof(serial));
if (rv != SECSuccess) {
return nullptr;
}
UniqueCERTCertificate certificate(
CERT_CreateCertificate(serial, subject_name.get(), validity.get(),
certreq.get()));
if (!certificate) {
return nullptr;
}
PLArenaPool *arena = certificate->arena;
rv = SECOID_SetAlgorithmID(arena, &certificate->signature,
SEC_OID_ANSIX962_ECDSA_SHA256_SIGNATURE, 0);
if (rv != SECSuccess)
return nullptr;
// Set version to X509v3.
*(certificate->version.data) = SEC_CERTIFICATE_VERSION_3;
certificate->version.len = 1;
SECItem innerDER;
innerDER.len = 0;
innerDER.data = nullptr;
if (!SEC_ASN1EncodeItem(arena, &innerDER, certificate.get(),
SEC_ASN1_GET(CERT_CertificateTemplate))) {
return nullptr;
}
SECItem *signedCert = PORT_ArenaZNew(arena, SECItem);
if (!signedCert) {
return nullptr;
}
rv = SEC_DerSignData(arena, signedCert, innerDER.data, innerDER.len,
private_key.get(),
SEC_OID_ANSIX962_ECDSA_SHA256_SIGNATURE);
if (rv != SECSuccess) {
return nullptr;
}
certificate->derCert = *signedCert;
RefPtr<DtlsIdentity> identity = new DtlsIdentity(Move(private_key),
Move(certificate),
ssl_kea_ecdh);
return identity.forget();
}
