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

python ros 设置机器人的位置

时间:2019-06-06 19:27:39      阅读:180      评论:0      收藏:0      [点我收藏+]

标签:bsp   euler   try   _id   header   anon   ade   math   nta   

 

#!/usr/bin/env python
import rospy
import math
from tf import transformations
from geometry_msgs.msg import PoseWithCovarianceStamped
class PoseSetter(rospy.SubscribeListener):
    def __init__(self, pose):
        self.pose = pose
    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
        p = PoseWithCovarianceStamped()
        p.header.frame_id = "map"
        p.pose.pose.position.x = self.pose[0]
        p.pose.pose.position.y = self.pose[1]
        (p.pose.pose.orientation.x,
        p.pose.pose.orientation.y,
        p.pose.pose.orientation.z,
        p.pose.pose.orientation.w) = transformations.quaternion_from_euler(0, 0, self.pose[2])
        p.pose.covariance[6*0+0] = 0.5 * 0.5
        p.pose.covariance[6*1+1] = 0.5 * 0.5
        p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
        peer_publish(p)
x=True
pose = [-9.983256,-2.641909,-1.201580]#x,y,a
rospy.init_node(‘pose_setter‘, anonymous=True)
pub=rospy.Publisher("initialpose", PoseWithCovarianceStamped, queue_size=10)
pub.publish(PoseSetter(pose))

 

python ros 设置机器人的位置

标签:bsp   euler   try   _id   header   anon   ade   math   nta   

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

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