// callback function for odom data void OdomEstimationNode::odomCallback(const OdomConstPtr& odom) { odom_callback_counter_++; ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec()); assert(odom_used_); // receive data boost::mutex::scoped_lock lock(odom_mutex_); odom_stamp_ = odom->header.stamp; odom_time_ = Time::now(); Quaternion q; tf::quaternionMsgToTF(odom->pose.pose.orientation, q); odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0)); for (unsigned int i=0; i<6; i++) for (unsigned int j=0; j<6; j++) odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j]; my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, "base_footprint", "wheelodom"), odom_covariance_); // activate odom if (!odom_active_) { if (!odom_initializing_){ odom_initializing_ = true; odom_init_stamp_ = odom_stamp_; ROS_INFO("Initializing Odom sensor"); } if ( filter_stamp_ >= odom_init_stamp_){ odom_active_ = true; odom_initializing_ = false; ROS_INFO("Odom sensor activated"); } else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.", (odom_init_stamp_ - filter_stamp_).toSec()); } #ifdef __EKF_DEBUG_FILE__ // write to file double tmp, yaw; odom_meas_.getBasis().getEulerZYX(yaw, tmp, tmp); odom_file_ << odom_meas_.getOrigin().x() << " " << odom_meas_.getOrigin().y() << " " << yaw << " " << endl; #endif };
// callback function for VO data void OdomEstimationNode::voCallback(const VoConstPtr& vo) { vo_callback_counter_++; assert(vo_used_); // get data boost::mutex::scoped_lock lock(vo_mutex_); vo_stamp_ = vo->header.stamp; vo_time_ = Time::now(); poseMsgToTF(vo->pose.pose, vo_meas_); for (unsigned int i=0; i<6; i++) for (unsigned int j=0; j<6; j++) vo_covariance_(i+1, j+1) = vo->pose.covariance[6*i+j]; my_filter_.addMeasurement(StampedTransform(vo_meas_.inverse(), vo_stamp_, "base_footprint", "vo"), vo_covariance_); // activate vo if (!vo_active_) { if (!vo_initializing_){ vo_initializing_ = true; vo_init_stamp_ = vo_stamp_; ROS_INFO("Initializing Vo sensor"); } if (filter_stamp_ >= vo_init_stamp_){ vo_active_ = true; vo_initializing_ = false; ROS_INFO("Vo sensor activated"); } else ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.", (vo_init_stamp_ - filter_stamp_).toSec()); } #ifdef __EKF_DEBUG_FILE__ // write to file double Rx, Ry, Rz; vo_meas_.getBasis().getEulerZYX(Rz, Ry, Rx); vo_file_ << vo_meas_.getOrigin().x() << " " << vo_meas_.getOrigin().y() << " " << vo_meas_.getOrigin().z() << " " << Rx << " " << Ry << " " << Rz << endl; #endif };
// 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"); } } };
// callback function for imu data void OdomEstimationNode::imuCallback(const ImuConstPtr& imu) { imu_callback_counter_++; assert(imu_used_); // receive data imu_stamp_ = imu->header.stamp; tf::Quaternion orientation; quaternionMsgToTF(imu->orientation, orientation); imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0)); for (unsigned int i=0; i<3; i++) for (unsigned int j=0; j<3; j++) imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j]; // Transforms imu data to base_footprint frame if (!robot_state_.waitForTransform("base_footprint", imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){ // warn when imu was already activated, not when imu is not active yet if (imu_active_) ROS_ERROR("Could not transform imu message from %s to base_footprint", imu->header.frame_id.c_str()); else if (my_filter_.isInitialized()) ROS_WARN("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str()); else ROS_DEBUG("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str()); return; } StampedTransform base_imu_offset; robot_state_.lookupTransform("base_footprint", imu->header.frame_id, imu_stamp_, base_imu_offset); imu_meas_ = imu_meas_ * base_imu_offset; imu_time_ = Time::now(); // manually set covariance untile imu sends covariance if (imu_covariance_(1,1) == 0.0){ SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0; measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec imu_covariance_ = measNoiseImu_Cov; } my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, "base_footprint", "imu"), imu_covariance_); // activate imu if (!imu_active_) { if (!imu_initializing_){ imu_initializing_ = true; imu_init_stamp_ = imu_stamp_; ROS_INFO("Initializing Imu sensor"); } if ( filter_stamp_ >= imu_init_stamp_){ imu_active_ = true; imu_initializing_ = false; ROS_INFO("Imu sensor activated"); } else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.", (imu_init_stamp_ - filter_stamp_).toSec()); } if (debug_){ // write to file double tmp, yaw; imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); imu_file_ << yaw << endl; } };