ROS 发布PoseWithCovarianceStamped 消息重新初始化位姿python

yiyamala

已于 2023-12-30 18:34:21 修改

阅读量899

点赞数 8

分类专栏： ROS机器人文章标签： python 开发语言机器人

于 2023-12-30 16:13:37 首次发布

本文链接：https://blog.csdn.net/yiyamala/article/details/135306483

版权

ROS机器人专栏收录该内容

4 篇文章 0 订阅

订阅专栏

从这个项目里摘抄修改的的

https://github.com/RGring/drl_local_planner_ros_stable_baselines/blob/master/rl_agent/src/rl_agent/env_utils/task_generator.py

上代码

#!/usr/bin/env python
# encoding: utf-8


import rospy
import math
from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist, Point
from std_msgs.msg import Int8
from move_base_msgs.msg import *
from pyquaternion import Quaternion
from tf import transformations


class TaskGenerator():

    def __init__(self):

        # Publisher
        self.initialpose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)


    def pub_initial_position(self, set_pose):
        rospy.loginfo("start test inital pose...")
        setpose_pub = rospy.Publisher("initialpose",PoseWithCovarianceStamped,latch=True, queue_size=10)
        rospy.loginfo("start set pose...")
        p = PoseWithCovarianceStamped()
        p.header.stamp = rospy.Time.now()
        p.header.frame_id = "map"
        p.pose.pose.position.x = set_pose['x']
        p.pose.pose.position.y = set_pose['y']
        p.pose.pose.position.z = set_pose['a']

        (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, set_pose['a'])
        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

        setpose_pub.publish(p) # 有时候发一次不行，原因不明
        rospy.sleep(1)
        setpose_pub.publish(p)
        rospy.sleep(1)

   

if __name__ == '__main__':

    rospy.init_node('test_initalpose', anonymous=False)
    task = TaskGenerator()
    set_pose = {'x':13.767,'y':-10.380,'a':3.127}
    task.pub_initial_position(set_pose)