Turbot与python编程-实现多点拍照

Turbot与python教程-实现多点拍照

说明：

• 介绍如何实现通过python控制turbot实现多点拍照

代码：

• 参考代码：github
• 实现代码：

import rospy
import yaml
from take_photo import TakePhoto
from go_to_specific_point_on_map import GoToPose

if __name__ == '__main__':

    # Read information from yaml file
    with open("route.yaml", 'r') as stream:
        dataMap = yaml.load(stream)

    try:
        # Initialize
        rospy.init_node('follow_route', anonymous=False)
        navigator = GoToPose()
        camera = TakePhoto()

        for obj in dataMap:

            if rospy.is_shutdown():
                break

            name = obj['filename']

            # Navigation
            rospy.loginfo("Go to %s pose", name[:-4])
            success = navigator.goto(obj['position'], obj['quaternion'])
            if not success:
                rospy.loginfo("Failed to reach %s pose", name[:-4])
                continue
            rospy.loginfo("Reached %s pose", name[:-4])

            # Take a photo
            if camera.take_picture(name):
                rospy.loginfo("Saved image " + name)
            else:
                rospy.loginfo("No images received")

            rospy.sleep(1)

    except rospy.ROSInterruptException:
        rospy.loginfo("Ctrl-C caught. Quitting")

演示：

• 主机，新终端，启动底盘

$ roslaunch turbot_bringup minimal.launch

• 主机，新终端，启动摄像头

$ roslaunch turbot_bringup kinect.launch

• 配置route.yaml文件

$ rosed turbot_code route.yaml

• 文件内容如下，参考github：

- {filename: 'dumpster.png', position: { x: 0.355, y: -0.2}, quaternion: {r1: 0, r2: 0, r3: -0.628, r4: 0.778}}
- {filename: 'cube_20k.png', position: { x: 2.843, y: -1.743}, quaternion: {r1: 0, r2: 0, r3: 0.936, r4: 0.353}}
- {filename: 'bookshelf.png', position: { x: 1.401, y: 2.913}, quaternion: {r1: 0, r2: 0, r3: 0.904, r4: -0.427}}

• 修改对应的目标点，图片名称

• 从机，新终端，启动脚本

$ rosrun turbot_code followTheRoute.py

• 脚本启动后，会逐个目标导航，并拍下图片

参考：

• http://www.ncnynl.com/archives/201609/823.html