TEST(TimeCache, CartesianInterpolation) { uint64_t runs = 100; double epsilon = 1e-6; seed_rand(); tf::TimeCache cache; std::vector<double> xvalues(2); std::vector<double> yvalues(2); std::vector<double> zvalues(2); uint64_t offset = 200; StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step])); t.stamp_ = ros::Time().fromNSec(step * 100 + offset); TransformStorage stor(t, 2, 0); cache.insertData(stor); } TransformStorage out; for (int pos = 0; pos < 100 ; pos++) { cache.getData(ros::Time().fromNSec(offset + pos), out); double x_out = out.translation_.x(); double y_out = out.translation_.y(); double z_out = out.translation_.z(); EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon); EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon); EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon); } cache.clearList(); } }
void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame, const Time& time, StampedTransform& transform) const { std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame); std::string mapped_src = assert_resolved(tf_prefix_, source_frame); if (mapped_tgt == mapped_src) { transform.setIdentity(); transform.child_frame_id_ = mapped_src; transform.frame_id_ = mapped_tgt; transform.stamp_ = now(); return; } boost::recursive_mutex::scoped_lock lock(frame_mutex_); CompactFrameID target_id = lookupFrameNumber(mapped_tgt); CompactFrameID source_id = lookupFrameNumber(mapped_src); std::string error_string; TransformAccum accum; int retval = walkToTopParent(accum, time, target_id, source_id, &error_string); if (retval != NO_ERROR) { switch (retval) { case CONNECTIVITY_ERROR: throw ConnectivityException(error_string); case EXTRAPOLATION_ERROR: throw ExtrapolationException(error_string); case LOOKUP_ERROR: throw LookupException(error_string); default: fprintf(stderr,"Unknown error code: %d\n", retval); break; } } transform.setOrigin(accum.result_vec); transform.setRotation(accum.result_quat); transform.child_frame_id_ = mapped_src; transform.frame_id_ = mapped_tgt; transform.stamp_ = accum.time; };
void currentSweepHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn) { pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZHSV>()); pcl::fromROSMsg(*laserCloudIn, *laserCloud); gLaserOdometry->addCurrentSweep(laserCloud, laserCloudIn->header.stamp.toSec()); // Translation and Rotation of the calculated Odometry double tx = 0; double ty = 0; double tz = 0; double rx = 0; double ry = 0; double rz = 0; // Publish the calculated odometry via TF StampedTransform laserOdometry; laserOdometry.setOrigin(Vector3(tx, ty, tz)); laserOdometry.setRotation(createQuaternionFromRPY(rz, -rx, -ry)); laserOdometry.frame_id_ = "/camera_init"; laserOdometry.child_frame_id_ = "/camera"; laserOdometry.stamp_ = laserCloudIn->header.stamp; tfBroadcaster->sendTransform(laserOdometry); }