mirror of
https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
synced 2023-05-28 12:51:38 +08:00
36 lines
1.2 KiB
Python
Executable File
36 lines
1.2 KiB
Python
Executable File
#!/usr/bin/env python2
|
|
# coding=utf8
|
|
from __future__ import print_function, division, absolute_import
|
|
|
|
import argparse
|
|
|
|
import rospy
|
|
import tf.transformations
|
|
from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped
|
|
|
|
if __name__ == '__main__':
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument('x', type=float)
|
|
parser.add_argument('y', type=float)
|
|
parser.add_argument('z', type=float)
|
|
parser.add_argument('yaw', type=float)
|
|
parser.add_argument('pitch', type=float)
|
|
parser.add_argument('roll', type=float)
|
|
args = parser.parse_args()
|
|
|
|
rospy.init_node('publish_initial_pose')
|
|
pub_pose = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)
|
|
|
|
# 转换为pose
|
|
quat = tf.transformations.quaternion_from_euler(args.roll, args.pitch, args.yaw)
|
|
xyz = [args.x, args.y, args.z]
|
|
|
|
initial_pose = PoseWithCovarianceStamped()
|
|
initial_pose.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
|
|
initial_pose.header.stamp = rospy.Time().now()
|
|
initial_pose.header.frame_id = 'map'
|
|
rospy.sleep(1)
|
|
rospy.loginfo('Initial Pose: {} {} {} {} {} {}'.format(
|
|
args.x, args.y, args.z, args.yaw, args.pitch, args.roll, ))
|
|
pub_pose.publish(initial_pose)
|