void handle_odom(const nav_msgs::Odometry::ConstPtr& msg)
{
tf::Transform transform;
poseMsgToTF(msg->pose.pose, transform);
tf::StampedTransform stamped_transform(transform, msg->header.stamp, msg->header.frame_id, msg->child_frame_id);
tf_br.sendTransform(stamped_transform);
}
// 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
};
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) {
tf::Transform transform;
poseMsgToTF(req->pose, transform);
send_attitude_transform(transform, req->header.stamp);
}
void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req) {
tf::Transform transform;
poseMsgToTF(req->pose.pose, transform);
send_vision_transform(transform, req->header.stamp);
}