码迷,mamicode.com
首页 > 编程语言 > 详细

python ros 订阅robot_pose获取机器人位置

时间:2019-01-09 01:20:26      阅读:830      评论:0      收藏:0      [点我收藏+]

标签:geo   display   previous   ros   until   orm   isp   机器人   rev   

 

#!/usr/bin/env python
import rospy
import tf
from tf.transformations import *
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion


def get_pos(data):
    (roll, pitch, yaw) = euler_from_quaternion([data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w])
    rospy.loginfo("current position(x:%f,y:%f,z:%f),theta:%f", data.position.x, data.position.y, data.position.z, yaw)
    #rospy.loginfo("current position(x:%f,y:%f,z:%f)", data.position.x, data.position.y, data.position.z)


def poslistener():
    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our listener node so that multiple listeners can
    # run simultaneously.
    rospy.init_node(poslistener, anonymous=True)

    rospy.Subscriber("robot_pose", Pose, get_pos)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()


if __name__ == __main__:
    poslistener()

 

python ros 订阅robot_pose获取机器人位置

标签:geo   display   previous   ros   until   orm   isp   机器人   rev   

原文地址:https://www.cnblogs.com/sea-stream/p/10242112.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!