导航中通过“initialpose“设置机器人位姿

rviz中 2D Pose Estimate如何设定机器人的位姿
amcl功能包中
amcl_node.cpp
initial_pose_sub_ = nh_.subscribe(“initialpose”, 2, &AmclNode::initialPoseReceived, this);
也就是说我们需要发布话题"initialpose"
运行导航功能查看"initialpose"的消息类型

rostopic type /initialpose

消息类型为"geometry_msgs/PoseWithCovarianceStamped"，查看消息数据格式

~$ rosmsg show geometry_msgs/PoseWithCovarianceStamped 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance

编写"initialpose"话题发布节点参考：
C++实现RVIZ 2D POSE ESTIMATE 功能设置机器人初始坐标
python版本

#!/usr/bin/env python
#coding:utf-8
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from std_msgs.msg import Header
import math


def pose_pub(alpha,x_pos,y_pos):

    initial_pose_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=1)

    pose_msg=PoseWithCovarianceStamped()
    pose_msg.header.frame_id = "map"
    pose_msg.pose.pose.position.x = x_pos
    pose_msg.pose.pose.position.y = y_pos
    pose_msg.pose.covariance[0] = 0.25
    pose_msg.pose.covariance[6 * 1 + 1] = 0.25
    pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942
    pose_msg.pose.pose.orientation.z = math.sin(alpha/2)
    pose_msg.pose.pose.orientation.w = math.cos(alpha/2)

    initial_pose_pub.publish(pose_msg)

if __name__ == '__main__':
    #初始化节点
    rospy.init_node("initialpose_node")
    alpha = 0
    x_pos = 0
    y_pos = 0
    rate = rospy.Rate(10) # 10hz  
    while not rospy.is_shutdown():
        pose_pub(alpha,x_pos,y_pos)
        rate.sleep()