背景
最近部署了基于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
<package>
<name>elastic_bridge</name>
<version>0.0.0</version>
<description>The elastic_bridge package</description>
<maintainer email="zh@todo.todo">smile</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
</package>
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.
注意事项2
-
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.