cartographer建图，重定位及发布消息结构为nav_msgs::Odometry的odom话题

完整功能包（包含carto建图，重定位以及odom话题发布）上传至https://download.csdn.net/download/zhaohaowu/33647981
众所周知，cartographer的重定位模式没有发布消息结构为nav_msgs::Odometry的odom话题
但cartographer_ros功能包中包含tracked_pose话题，里面包含了map坐标系下雷达的位姿，
默认不打开，我们在此基础上完成odom话题的发布

首先找到node.cc里面的node_options_.publish_tracked_pose，可以看到如果if为true，将发布tracked_pose话题，选中publish_tracked_pose按f12，跳转到定义处，在node_options.h文件下，可以看到默认为false，改为true，这样cartographer将发布tracked_pose话题

然后，两种方法发布odom

• 方法一，在node.cc中发布

在node.h中
ctrl+f搜索tracked_pose_publisher_
在其下面添加 ::ros::Publisher pub;
在node.cc中，
第一个if (node_options_.publish_tracked_pose) 中加入

pub = node_handle_.advertise<::nav_msgs::Odometry>("carto_odom", 100);

第二个if (node_options_.publish_tracked_pose) 中加入

        current_time = ros::Time::now();
        current_x = pose_msg.pose.position.x;
        current_yaw = tf::getYaw(pose_msg.pose.orientation);
        dt = (current_time - last_time).toSec();
        dx = current_x - last_x;
        d_yaw = current_yaw - last_yaw;
        lin_speed = dx/dt;
        ang_speed = d_yaw/dt;
        ::nav_msgs::Odometry msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "odom";
        msg.child_frame_id = "rslidar";
        msg.pose.pose.position = pose_msg.pose.position;
        msg.pose.pose.orientation = pose_msg.pose.orientation;
        msg.twist.twist.linear.x = lin_speed;
        msg.twist.twist.linear.y = 0.0;
        msg.twist.twist.angular.z = ang_speed;
        pub.publish(msg);
        last_time = ros::Time::now();
        last_x = pose_msg.pose.position.x;
        last_yaw = tf::getYaw(pose_msg.pose.orientation);

• 方法二，使用rosparam传递参数，另外写个节点

在node.cc中
第二个if (node_options_.publish_tracked_pose) 中加入

node_handle_.setParam("position_x", pose_msg.pose.position.x);
node_handle_.setParam("position_y", pose_msg.pose.position.y);
node_handle_.setParam("orientation_x", pose_msg.pose.orientation.x);
node_handle_.setParam("orientation_y", pose_msg.pose.orientation.y);
node_handle_.setParam("orientation_z", pose_msg.pose.orientation.z);
node_handle_.setParam("orientation_w", pose_msg.pose.orientation.w);

在新节点中，
carto_odom.cpp

#include
#include
#include
#include

int main(int argc, char** argv)
{
    ros::init(argc, argv, "carto_odom");
    ros::NodeHandle(n);
    ros::Publisher pub = n.advertise<nav_msgs::Odometry>("carto_odom_test", 1);
    double position_x, position_y, orientation_x, orientation_y, orientation_z, orientation_w;
    nav_msgs::Odometry tmp;

    ros::Time current_time, last_time;
    double current_x, last_x, current_yaw, last_yaw, lin_speed, ang_speed;
    while (ros::ok())
    {
        n.getParam("position_x",position_x);
        n.getParam("position_y",position_y);
        n.getParam("orientation_x",orientation_x);
        n.getParam("orientation_y",orientation_y);
        n.getParam("orientation_z",orientation_z);
        n.getParam("orientation_w",orientation_w);
        current_time = ros::Time::now();
        current_x = position_x;
        tmp.pose.pose.orientation.x = orientation_x;
        tmp.pose.pose.orientation.y = orientation_y;
        tmp.pose.pose.orientation.z = orientation_z;
        tmp.pose.pose.orientation.w = orientation_w;
        current_yaw = tf::getYaw(tmp.pose.pose.orientation);
        nav_msgs::Odometry msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "odom";
        msg.child_frame_id = "rslidar";
        msg.pose.pose.position.x = position_x;
        msg.pose.pose.position.y = position_y;
        msg.pose.pose.orientation.x = orientation_x;
        msg.pose.pose.orientation.y = orientation_y;
        msg.pose.pose.orientation.z = orientation_z;
        msg.pose.pose.orientation.w = orientation_w;
        lin_speed = (current_x - last_x)/((current_time - last_time).toSec());
        ang_speed = (current_yaw - last_yaw)/((current_time - last_time).toSec());
        msg.twist.twist.linear.x = lin_speed;
        msg.twist.twist.angular.z = ang_speed;
        pub.publish(msg);
        n.getParam("position_x",position_x);
        n.getParam("position_y",position_y);
        n.getParam("orientation_x",orientation_x);
        n.getParam("orientation_y",orientation_y);
        n.getParam("orientation_z",orientation_z);
        n.getParam("orientation_w",orientation_w);
        last_time = ros::Time::now();
        last_x = position_x;
        tmp.pose.pose.orientation.x = orientation_x;
        tmp.pose.pose.orientation.y = orientation_y;
        tmp.pose.pose.orientation.z = orientation_z;
        tmp.pose.pose.orientation.w = orientation_w;
        last_yaw = tf::getYaw(tmp.pose.pose.orientation);
        usleep(80 * 1000);
    }
    ros::spin();
    std::cout << "\n\n结束odom发布\n\n";
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(carto_test)

add_compile_options(-std=c++11)
SET(CMAKE_BUILD_TYPE Debug)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  nav_msgs
  tf
  # message_generation
)
add_message_files(
  FILES
  position.msg
)
generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS roscpp nav_msgs tf
  # message_runtime
)

find_package(OpenCV  REQUIRED)
find_package(aruco  REQUIRED )

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  ${aruco_INCLUDE_DIRS}
)

add_executable(carto_odom src/carto_odom.cpp)
target_link_libraries(carto_odom
  ${catkin_LIBRARIES}
  ${OpenCV_LIBS}
  ${aruco_LIBS}
)
add_dependencies(aruco_test ${PROJECT_NAME}_gencpp)

Original: https://blog.csdn.net/zhaohaowu/article/details/120889458
Author: dear小王子
Title: cartographer建图，重定位及发布消息结构为nav_msgs::Odometry的odom话题