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