move_base_simple/goal 使用记录BUG

无论使用rospy roscpp 发布话题move_base_simple/goal ，第一次发布的消息，基本都回丢。

具体原因不详。经排查，move_base_simple/goal的回调函数的消息队列长度为1，但调为10，依旧没有解决。

故先发一个空消息，休眠后再发正确的消息。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time
from geometry_msgs.msg import PoseStamped
 
def send_goal(x, y, theta):
    # 初始化节点
    rospy.init_node('send_goal_node', anonymous=True)
 
    # 创建一个发布者，发布目标位置消息
    pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
    
    
    # 创建一个PoseStamped消息对象，填充位置信息
    goal = PoseStamped()
    #先发送一个空位置，否则第一个包容易丢，具体原因可能是因为回调函数的缓冲长度为1
    pub.publish(goal) 
    time.sleep(1)
    
    goal = PoseStamped()
    goal.header.stamp = rospy.Time.now()
    goal.header.frame_id = "map"
    goal.pose.position.x = x
    goal.pose.position.y = y
    goal.pose.orientation.w = theta
 
    # 发布目标位置消息
    pub.publish(goal)
 
    # 等待一段时间，让小车到达目标位置
    rospy.sleep(5)
 
if __name__ == '__main__':
    # 发送目标位置为 x=1.0, y=1.0, theta=0.0 的消息
    send_goal(1.0, 0.0, 1.0)
    rospy.spin()