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