1 发布里程计
前期准备
pub_odometry = n.advertise<nav_msgs::Odometry>("odometry", 1000);
Eigen::Vector3d tmp_P;
Eigen::Quaterniond tmp_Q;
Eigen::Vector3d tmp_V;
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
发布显示
/**
* 发布最新的由imu直接递推得到的PQV
*/
void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header)
{
Eigen::Quaterniond quadrotor_Q = Q ;
nav_msgs::Odometry odometry;
odometry.header = header;
odometry.header.frame_id = "world";
odometry.pose.pose.position.x = P.x();
odometry.pose.pose.position.y = P.y();
odometry.pose.pose.position.z = P.z();
odometry.pose.pose.orientation.x = quadrotor_Q.x();
odometry.pose.pose.orientation.y = quadrotor_Q.y();
odometry.pose.pose.orientation.z = quadrotor_Q.z();
odometry.pose.pose.orientation.w = quadrotor_Q.w();
odometry.twist.twist.linear.x = V.x();
odometry.twist.twist.linear.y = V.y();
odometry.twist.twist.linear.z = V.z();
pub_latest_odometry.publish(odometry);
}