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