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