def SendInitialPose(InitialPosePublisher, initial_pose, time_stamp):
# goal: [x, y, yaw]
InitialPoseMsg = PoseWithCovarianceStamped()
#InitialPoseMsg.header.seq = 0
InitialPoseMsg.header.stamp = time_stamp
InitialPoseMsg.header.frame_id = 'map'
InitialPoseMsg.pose.pose.position.x = initial_pose[0]
InitialPoseMsg.pose.pose.position.y = initial_pose[1]
#InitialPoseMsg.pose.position.z = 0.0
quaternion = quaternion_from_euler(0, 0, initial_pose[2])
InitialPoseMsg.pose.pose.orientation.x = quaternion[0]
InitialPoseMsg.pose.pose.orientation.y = quaternion[1]
InitialPoseMsg.pose.pose.orientation.z = quaternion[2]
InitialPoseMsg.pose.pose.orientation.w = quaternion[3]
InitialPosePublisher.publish(InitialPoseMsg)
#========================================
评论列表
文章目录