ROS python 开始\停止导航、发布目标点、获取实时位置

在四个终端中分别执行以下命令，开启仿真
需要先有对应的包

roscore
roslaunch rbx1_bringup fake_turtlebot.launch 开启仿真
roslaunch rbx1_nav fake_move_base_blank_map.launch 加载空地图
rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz 开启rviz

发布目标点，发送后立即开始运动
命令行形式
PoseStamped数据类型，主要要指明frame_id，position（x\y\z ），
orientation（四元数）
frame_id 可以选‘map’(世界坐标系）、‘base_link’（小车的坐标系）

rostopic pub /move_base_simple/goal  geometry_msgs/PoseStamped \
 '{header: {frame_id: "map"},pose: {position:{x: 1.0,y: 0,z: 0},orientation: {x: 0,y: 0,z: 0,w: 1}}}'

python 形式

#! /usr/bin/env python

import rospy
import time
from geometry_msgs.msg import PoseStamped

if __name__ == '__main__':
    rospy.init_node('pubpose')
    turtle_vel_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)
    
    mypose=PoseStamped()
    turtle_vel_pub.publish(mypose) #先发送一个空位置，试探一下，否则第一个包容易丢
    time.sleep(1)
    
    mypose=PoseStamped()
    mypose.header.frame_id='base_link' #设置自己的目标
    mypose.pose.position.x=1
    mypose.pose.position.y=0
    mypose.pose.position.z=0
    mypose.pose.orientation.x=0
    mypose.pose.orientation.y=0
    mypose.pose.orientation.z=1
    mypose.pose.orientation.w=0
    
    turtle_vel_pub.publish(mypose) #发送自己设置的目标点

    time.sleep(5)

取消目标点，停止导航
即给/move_base/cancel 节点发送GoalID

rostopic pub /move_base/cancel actionlib_msgs/GoalID -- {}

实时获取小车当前位置即获取tf
参考wiki tf listener

    #!/usr/bin/env python
import rospy
import tf
if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener() #开始监听tf
    while not rospy.is_shutdown():
         try:
             (trans,rot) = listener.lookupTransform('map', 'base_link', rospy.Time(0)) #获取'base_link'相对于'map'的坐标变换，返回两个列表
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
             continue
         print('position',trans)
         print('orient',rot)
         time.sleep(0.5)

发布当前位置作为目标点，也可以终止导航