【cartographer without ros】四、里程计odom数据转换

上一节介绍了cartographer中的TimedPointCloudData数据相关转换。

本节将介绍里程计Odom数据转换为cartographer中的OdometryData数据。还有cartographer中的OdometryData数据和Ros的nav_msgs::Odometry数据相互转换，方便引入Rosbag进行储存和读取。

---

目录

1：OdometryData数据类型

2：Odom数据转换cartographer数据

3：Odom数据转换Ros数据

4：cartographer数据转换Ros数据

5：Ros数据转换cartographer数据

---

1：OdometryData数据类型

【odometry_data.h】中查看数据结构：

//时间
common::Time time;
//位置
transform::Rigid3d pose;

---

2：Odom数据转换cartographer数据

cartographer::sensor::OdometryData GetOdometryData()
{
    cartographer::sensor::OdometryData odometry_data;

    current_time = ros::Time::now();
    double dt = (current_time - last_time).toSec();
    double dx = (vx * cos(th) - vy * sin(th)) * dt;
    double dy = (vx * sin(th) + vy * cos(th)) * dt;
    double dth = vth * dt;
    last_time = current_time;

    x += dx;
    y += dy;
    th += dth;


    cartographer::common::Time curTime = cartographer::common::NowTime();
    const Eigen::AngleAxisd rotation(th, Eigen::Vector3d::UnitZ());
    const Eigen::Vector3d translation(x, y, 0.0);
    cartographer::transform::Rigid3d pose(translation, rotation);
    odometry_data = cartographer::sensor::OdometryData{curTime, pose};

    return odometry_data;
}

---

3：Odom数据转换Ros数据

【cartographer_ros】四： 发布和订阅里程计odom信息

---

4：cartographer数据转换Ros数据

nav_msgs::Odometry ToRosMsg(cartographer::sensor::OdometryData odometry_data)
{
    nav_msgs::Odometry odom;

    odom.header.stamp = TimeRosFromCarto(odometry_data.time);
    odom.header.frame_id = "odom";
    odom.pose.pose.position.x = odometry_data.pose.translation().x();
    odom.pose.pose.position.y = odometry_data.pose.translation().y();
    odom.pose.pose.position.z = odometry_data.pose.translation().z();
    odom.pose.pose.orientation.w = odometry_data.pose.rotation().w();
    odom.pose.pose.orientation.x = odometry_data.pose.rotation().x();
    odom.pose.pose.orientation.y = odometry_data.pose.rotation().y();
    odom.pose.pose.orientation.z = odometry_data.pose.rotation().z();
    odom.child_frame_id = "base_link";
    return odom;
}

---

5：Ros数据转换cartographer数据

cartographer::sensor::OdometryData ToCartoSensor(nav_msgs::Odometry odom)
{
    cartographer::sensor::OdometryData odometry_data;
    odometry_data.time = Time::CartoFromRos(odom.header.stamp);
    const Eigen::Vector3d translation(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z);
    const Eigen::Quaterniond rotation(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z);
    odometry_data.pose = cartographer::transform::Rigid3d(translation, rotation);
    return odometry_data;
}

【完】

---

下一节会介绍里程计Imu数据转换为cartographer中的cartographer::sensor::ImuData数据。