ROS相关：使用rospy 编写ros程序并使用rosbag存储数据

为什么使用rospy

ROS支持C++和Python，由于ROS的底层是由C++编写，因此大多数的ROS程序都使用C++，但是Python语言接口简单，更容易编写。并且可以使用python与深度学习的一些框架比如Caffe，TensorFlow，Theano等结合。因此，采用python是更好的选择。本文只总结一些rospy使用的细节和rosbag的使用

rospy的优点

除了上面说的接口简单，容易编写，与深度学习框架相容之外，还有一点就是使用python编写的程序可以不需要catkin_make就可以运行。对于用过ros的童鞋恐怕都知道为了编译成功还需要修改CMakeLists.txt文件，非常麻烦，常常编译不成功。那么使用python写ROS程序会发现超级简单。写完直接运行。

rospy处理image的方法

该程序转自stackoverflow

# rospy for the subscriber
import rospy
# ROS Image message
from sensor_msgs.msg import Image
# ROS Image message -> OpenCV2 image converter
from cv_bridge import CvBridge, CvBridgeError
# OpenCV2 for saving an image
import cv2

# Instantiate CvBridge
bridge = CvBridge()

def image_callback(msg):
    print("Received an image!")
    try:
        # Convert your ROS Image message to OpenCV2
        cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
    except CvBridgeError, e:
        print(e)
    else:
        # Save your OpenCV2 image as a jpeg 
        cv2.imwrite('camera_image.jpeg', cv2_img)

def main():
    rospy.init_node('image_listener')
    # Define your image topic
    image_topic = "/cameras/left_hand_camera/image"
    # Set up your subscriber and define its callback
    rospy.Subscriber(image_topic, Image, image_callback)
    # Spin until ctrl + c
    rospy.spin()

if __name__ == '__main__':
    main()

基本上从上面的代码可以看出使用rospy编程的方法。

如何导入message

有时候有一些特定的message需要导入，一般是有一个msg文件。

所以，比如要导入turtlesim/Pose的message类型，那么在python中是这样：

from turtlesim.msg import Pose

更通用的说如要导入某个自己编写的特定格式 xxx/kkk 那么代码为：

from xxx.msg import kkk

rosbag 的使用

rosbag用来记录数据，有两种方法，一种是在命令行中进行。比如：

rosbag record -O dataset /turtle1/pose /turtle1/cmd_vel

另外一种做法是使用rospy。

import rospy
import rosbag
from turtlesim.msg import Pose
from turtlesim.msg import Color

bag = rosbag.Bag('dataset.bag','w')

def processColorData(data):
    bag.write('Color',data)

def processPoseData(data):
    bag.write('Pose',data)

def main():
    rospy.init_node('drone_track_data_saver')
    rospy.Subscriber('/turtle1/color_sensor',Color,processColorData)
    rospy.Subscriber('/turtle1/pose',Pose,processPoseData)

    rate = rospy.Rate(1) # 10hz
    while not rospy.is_shutdown():
        rate.sleep()
    else:
        bag.close()

if __name__ == '__main__':
        main()

两者的效果是一样的。采用python来写的话可以增加一些特定的控制，比如时间间隔。

使用python读取rosbag的内容也同样非常简单：

import rosbag

rbag = rosbag.Bag('dataset.bag')
numbers = 100
for topic,msg,t in rbag.read_messages():
        if numbers < 1:
            break
        numbers -= 1
        print topic,msg
rbag.close()