Python订阅自定义ROS topic并读取message

Python订阅自定义ROS topic并读取message 背景

最近部署了基于ROS的实时稠密场景建图, 参考Elastic_bridge. 现在需要用python获取相机每一时刻的pose。该pose的消息类型定义在frameState.msg 文件中。具体内容如下

uint64 seq
uint64 width
uint64 height

# intrinsics, in pixels
float32 focal_x
float32 focal_y
float32 center_x
float32 center_y

# current sensor pose
geometry_msgs/Pose pose

# SYNTHETIC, generated by ElasticFusion
float32[] color    # 3 float per pixel
float32[] depth    # 1 float per pixel
float32[] position # 3 float per pixel

uint64[] guid         # 1 guid per pixel
uint32[] luid         # 1 luid per pixel
uint32[] luid_removed # luids of surfels removed in this frame
uint32 max_luid

float32[] normal  # 3 float per pixel
float32[] radius  # 1 float per pixel

# INPUT for this frame, from the RGB-D sensor
uint8[] input_color   # RGB, 3 byte per pixel
uint16[] input_depth  # DEPTH, in mm

目录结构

创建如下的文件结构。
catkin_ws/
├── src
│ ├── msg
│ │ └── frameState.msg
│ ├── listener.py
│ ├── CMakeLists.txt
│ └── package.xml
└──
其中CMakeLists.txt和package.xml参考了其他博客的写法。如下：

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(elastic_bridge)

find_package(catkin REQUIRED COMPonENTS
  rospy
  std_msgs
  geometry_msgs
  trajectory_msgs
  message_generation
)

add_message_files(
  FILES
  frameState.msg
)

generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
trajectory_msgs
elastic_bridge
)

catkin_package(
#INCLUDE_DIRS include
#LIBRARIES test_py
CATKIN_DEPENDS rospy message_runtime
DEPENDS system_lib
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

package.xml

  elastic_bridge
  0.0.0
  The elastic_bridge package

  smile

  TODO

  catkin

  rospy
  std_msgs
  message_generation

  rospy
  std_msgs
  message_runtime

listener.py

# !/usr/bin/env python

import rospy
from elastic_bridge.msg import frameState


def callback(data):
    rospy.loginfo('Debug info')
    rospy.loginfo(data.pose)

def listener():
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber('/elastic_frame_state_stable', frameState, callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

编译workspace

按如上目录结构设置好代码之后，对python脚本添加可执行权限

chmod +x src/listener.py

然后切换到~/catkin_ws目录下编译, 编译完成后会生成build和devel文件夹

cd ~/catkin_ws
catkin_make

完成后按照如下命令运行即可。最终我们可以通过python订阅到ROS topic发送过来的消息

source devel/setup.bash
python listerner.py

注意事项

• package.xml和CMakeLists.txt中project的名字要和想订阅的topic对应的工程名相同。也可能不需要，但是在这个项目中如果随意指定project名字的话会报错。报错信息如下

  [ERROR] [1319162712.616980036]: Client [/clientNodeA] wants topic /server_msgs/Swarm 
  to have datatype/md5sum [server_msgs/Swarm/6a727e7bfd7e4aadf9d23c4a779b268e], 
  but our version has [geometry_msgs/Point/4a842b65f413084dc2b10fb484ea7f17]. 
  Dropping connection.