ROS里程计消息nav_msgs/Odometry的可视化方法

ROS中里程计的消息类型为nav_msgs/Odometry，该消息类型具有以下结构：

可以看到，里程计消息中的pose包含了位置pose.position和姿态pose.orientation

在ROS中，有一种常用消息类型为nav_msgs/Path，可视化的方法为：

①在一个节点中订阅发布的里程计话题消息nav_msgs/Odometry

②创建geometry_msgs::PoseStamped对象接收里程计的位姿

③创建nav_msgs/Path对象作为容器，将赋值后的对象push_back进nav_msgs/Path中并发布

然后即可在rviz中订阅包含nav_msgs/Path的话题并可视化轨迹

1.新建ROS工作空间

mkdir -p path_ws/src
cd path_ws
catkin_make
cd src
catkin_create_pkg path_3d roscpp rospy std_msgs nav_msgs geometry_msgs

2.在path_3d/src中编写消息收发节点文件path_3d.cpp

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>

nav_msgs::Path  path;
ros::Publisher  path_pub;
 
void pathCallback(const nav_msgs::Odometry::ConstPtr& odom_3d)
{
    geometry_msgs::PoseStamped position_3d;
    position_3d.pose.position.x = odom_3d->pose.pose.position.x; 
    position_3d.pose.position.y = odom_3d->pose.pose.position.y; 
    position_3d.pose.position.z = odom_3d->pose.pose.position.z;
    position_3d.pose.orientation = odom_3d->pose.pose.orientation;
 

    position_3d.header.stamp = odom_3d->header.stamp;
    position_3d.header.frame_id = "map";
 
    path.poses.push_back(position_3d);
    path.header.stamp = position_3d.header.stamp;
    path.header.frame_id = "map";
    path_pub.publish(path);
  
    std::cout << odom_3d -> header.stamp << ' ' << odom_3d->pose.pose.position.x << ' ' << odom_3d->pose.pose.position.y << ' ' << odom_3d->pose.pose.position.z << std::endl;
}
 
int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    ros::NodeHandle ph;
 
    path_pub = ph.advertise<nav_msgs::Path>("odom3d_path", 10, true);
    ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/odometry_3d", 10, pathCallback);  //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名，别忘了修改
    
    ros::Rate loop_rate(1000);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

3.path_3d文件夹目录中的CMakeLists.txt如下：

cmake_minimum_required(VERSION 2.8.3)
project(path_3d)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
message_generation
)

## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   geometry_msgs   std_msgs
 )

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES path_3d
  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs
  DEPENDS system_lib
)

include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)

add_executable(path_3d src/path_3d.cpp) #${PROJECT_NAME}_node
target_link_libraries(path_3d ${catkin_LIBRARIES}) # ${PROJECT_NAME}_node
add_dependencies(path_3d beginner_tutorials_generate_messages_cpp) #path_3d_node

4.在工作空间中编译功能包

// 打开命令行
// 进入工作空间最上层目录
cd path_ws
// 执行一下 source 命令
source devel/setup.bash
// 编译工作空间下的所有功能包
catkin_make
// 单独编译工作空间下的 path_3d 功能包
catkin_make -DCATKIN_WHITELIST_PACKAGES="path_3d"

5.启动节点、在rviz中订阅odom3d_path话题，即可查看可视化数据

// 终端中启动roscore
roscore
// 另一个终端启动节点
rosrun path_3d path_3d
// 在数据包路径下，播放带有 nav_msgs/Odometry 消息的数据包
rosbag play 你自己需要可视化的数据包.bag
// 第三个终端，启动rviz，frame选择map，订阅发布的odom3d_path话题
rviz