bool RobotNavigator::setCurrentPosition() { StampedTransform transform; try { mTfListener.lookupTransform(mMapFrame, mRobotFrame, Time(0), transform); }catch(TransformException ex) { ROS_ERROR("Could not get robot position: %s", ex.what()); return false; } double world_x = transform.getOrigin().x(); double world_y = transform.getOrigin().y(); double world_theta = getYaw(transform.getRotation()); unsigned int current_x = (world_x - mCurrentMap.getOriginX()) / mCurrentMap.getResolution(); unsigned int current_y = (world_y - mCurrentMap.getOriginY()) / mCurrentMap.getResolution(); unsigned int i; if(!mCurrentMap.getIndex(current_x, current_y, i)) { if(mHasNewMap || !getMap() || !mCurrentMap.getIndex(current_x, current_y, i)) { ROS_ERROR("Is the robot out of the map?"); return false; } } mStartPoint = i; mCurrentDirection = world_theta; mCurrentPositionX = world_x; mCurrentPositionY = world_y; return true; }
TEST(TimeCache, RepeatabilityRandomInsertOrder) { seed_rand(); tf::TimeCache cache; double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8}; std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); unsigned int runs = values.size(); StampedTransform t; t.setIdentity(); for ( uint64_t i = 0; i <runs ; i++ ) { values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.stamp_ = ros::Time().fromNSec(i); TransformStorage stor(t, i, 0); cache.insertData(stor); } TransformStorage out; for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), out); EXPECT_EQ(out.frame_id_, i); EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i)); } }
TEST(TimeCache, RepeatabilityReverseInsertOrder) { unsigned int runs = 100; seed_rand(); tf::TimeCache cache; std::vector<double> values(runs); StampedTransform t; t.setIdentity(); for ( int i = runs -1; i >= 0 ; i-- ) { values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.stamp_ = ros::Time().fromNSec(i); TransformStorage stor(t, i, 0); cache.insertData(stor); } TransformStorage out; for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), out); EXPECT_EQ(out.frame_id_, i); EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i)); } }
/** Constructor. * @param data initial stamped transform data * @param frame_id parent frame ID * @param child_frame_id child frame ID */ TransformStorage::TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id) : rotation(data.getRotation()) , translation(data.getOrigin()) , stamp(data.stamp) , frame_id(frame_id) , child_frame_id(child_frame_id) { }
void transformSubCallback(const tf::tfMessageConstPtr& msg) { r_limit_=p_limit_=y_limit_=distance_limit_=0.01; boost::mutex::scoped_lock l(m_mutex_pointCloudSubCallback); if(frame_id_.size()<1) { ROS_WARN("frame id is missing"); //frame_id_="head_cam3d_link"; return; } //pcl::PointCloud<Point>::Ptr pc = pcl::PointCloud<Point>::Ptr(new pcl::PointCloud<Point>); StampedTransform transform; /* 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; } */ try { //ROS_INFO("%s", frame_id_.c_str()); std::stringstream ss2; tf_listener_.waitForTransform("/map", frame_id_, ros::Time(0), ros::Duration(0.1)); tf_listener_.lookupTransform("/map", frame_id_, ros::Time(0), transform); KDL::Frame frame_KDL, frame_KDL_old; tf::TransformTFToKDL(transform, frame_KDL); tf::TransformTFToKDL(transform_old_, frame_KDL_old); double r,p,y; frame_KDL.M.GetRPY(r,p,y); double r_old,p_old,y_old; frame_KDL_old.M.GetRPY(r_old,p_old,y_old); if(trigger_always_ || first_ || fabs(r-r_old) > r_limit_ || fabs(p-p_old) > p_limit_ || fabs(y-y_old) > y_limit_ || transform.getOrigin().distance(transform_old_.getOrigin()) > distance_limit_) { if(triggerKeyFrame()) { transform_old_ = transform; first_=false; } } } catch (tf::TransformException ex) { ROS_ERROR("[keyframe_detector] : %s",ex.what()); } }
TEST(TimeCache, AngularInterpolation) { uint64_t runs = 100; double epsilon = 1e-6; seed_rand(); tf::TimeCache cache; std::vector<double> yawvalues(2); std::vector<double> pitchvalues(2); std::vector<double> rollvalues(2); uint64_t offset = 200; std::vector<btQuaternion> quats(2); StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0; pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); t.setRotation(quats[step]); t.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 TransformStorage stor(t, 3, 0); cache.insertData(stor); } TransformStorage out; for (int pos = 0; pos < 100 ; pos ++) { cache.getData(ros::Time().fromNSec(offset + pos), out); //get the transform for the position btQuaternion quat = out.rotation_; //get the quaternion out of the transform //Generate a ground truth quaternion directly calling slerp btQuaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); //Make sure the transformed one and the direct call match EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); } cache.clearList(); } }
TEST(TimeCache, ZeroAtFront) { uint64_t runs = 100; seed_rand(); tf::TimeCache cache; std::vector<double> values(runs); StampedTransform t; t.setIdentity(); for ( uint64_t i = 0; i <runs ; i++ ) { values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.stamp_ = ros::Time().fromNSec(i); TransformStorage stor(t, i, 0); cache.insertData(stor); } t.stamp_ = ros::Time().fromNSec(runs); TransformStorage stor(t, runs, 0); cache.insertData(stor); for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); } cache.getData(ros::Time(), stor); EXPECT_EQ(stor.frame_id_, runs); EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs)); t.stamp_ = ros::Time().fromNSec(runs+1); TransformStorage stor2(t, runs+1, 0); cache.insertData(stor2); //Make sure we get a different value now that a new values is added at the front cache.getData(ros::Time(), stor); EXPECT_EQ(stor.frame_id_, runs+1); EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1)); }
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 Transformer::lookupTransform(const std::string& target_frame,const Time& target_time, const std::string& source_frame, const Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const { tf::StampedTransform temp1, temp2; lookupTransform(fixed_frame, source_frame, source_time, temp1); lookupTransform(target_frame, fixed_frame, target_time, temp2); transform.setData( temp2 * temp1); transform.stamp_ = temp2.stamp_; transform.frame_id_ = target_frame; transform.child_frame_id_ = source_frame; };
TEST(TimeCache, DuplicateEntries) { TimeCache cache; StampedTransform t; t.setIdentity(); t.stamp_ = ros::Time().fromNSec(1); TransformStorage stor(t, 3, 0); cache.insertData(stor); cache.insertData(stor); cache.getData(ros::Time().fromNSec(1), stor); EXPECT_TRUE(!std::isnan(stor.translation_.x())); EXPECT_TRUE(!std::isnan(stor.translation_.y())); EXPECT_TRUE(!std::isnan(stor.translation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.x())); EXPECT_TRUE(!std::isnan(stor.rotation_.y())); EXPECT_TRUE(!std::isnan(stor.rotation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.w())); }
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); }
// filter loop void OdomEstimationNode::spin(const ros::TimerEvent& e) { ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec()); // check for timing problems if ( (odom_initializing_ || odom_active_) && (imu_initializing_ || imu_active_) ){ double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() ); if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff); } // initial value for filter stamp; keep this stamp when no sensors are active filter_stamp_ = Time::now(); // check which sensors are still active if ((odom_active_ || odom_initializing_) && (Time::now() - odom_time_).toSec() > timeout_){ odom_active_ = false; odom_initializing_ = false; ROS_INFO("Odom sensor not active any more"); } if ((imu_active_ || imu_initializing_) && (Time::now() - imu_time_).toSec() > timeout_){ imu_active_ = false; imu_initializing_ = false; ROS_INFO("Imu sensor not active any more"); } if ((vo_active_ || vo_initializing_) && (Time::now() - vo_time_).toSec() > timeout_){ vo_active_ = false; vo_initializing_ = false; ROS_INFO("VO sensor not active any more"); } // only update filter when one of the sensors is active if (odom_active_ || imu_active_ || vo_active_){ // update filter at time where all sensor measurements are available if (odom_active_) filter_stamp_ = min(filter_stamp_, odom_stamp_); if (imu_active_) filter_stamp_ = min(filter_stamp_, imu_stamp_); if (vo_active_) filter_stamp_ = min(filter_stamp_, vo_stamp_); // update filter if ( my_filter_.isInitialized() ) { bool diagnostics = true; if (my_filter_.update(odom_active_, imu_active_, vo_active_, filter_stamp_, diagnostics)){ // output most recent estimate and relative covariance my_filter_.getEstimate(output_); pose_pub_.publish(output_); ekf_sent_counter_++; // broadcast most recent estimate to TransformArray StampedTransform tmp; my_filter_.getEstimate(ros::Time(), tmp); if(!vo_active_) tmp.getOrigin().setZ(0.0); odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, "base_footprint")); if (debug_){ // write to file ColumnVector estimate; my_filter_.getEstimate(estimate); for (unsigned int i=1; i<=6; i++) corr_file_ << estimate(i) << " "; corr_file_ << endl; } } if (self_diagnose_ && !diagnostics) ROS_WARN("Robot pose ekf diagnostics discovered a potential problem"); } // initialize filer with odometry frame if ( odom_active_ && !my_filter_.isInitialized()){ my_filter_.initialize(odom_meas_, odom_stamp_); ROS_INFO("Kalman filter initialized with odom measurement"); } else if ( vo_active_ && !my_filter_.isInitialized()){ my_filter_.initialize(vo_meas_, vo_stamp_); ROS_INFO("Kalman filter initialized with vo measurement"); } } };