ros2- 7.4 导航应用开发指南

上一节我们是通过RVIZ2的工具给机器人指定的目标点，除了工具栏的工具，我们还可以使用代码的方式发送目标点给FishBot，本节就是调用接口进行控制和检测。

7.4.1 使用话题初始化机器人位姿

nav2 中，机器人在地图中位置是有amcl节点根据地图以及传感器数据进行计算的，但是最开始时，需要通过话题告知机器人得知位置，amcl才开始计算。

创建功能包fishbot_apllication,

ros2 pkg create fishbot_application --build-type ament_python --license Apache-2.0

src/fishbot_application/fishbot_application/目录下新建文件init_robot_pose.py

代码如下：

from setuptools import find_packages, setup

package_name = 'fishbot_application'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='bohu',
    maintainer_email='bohu83@163.com',
    description='TODO: Package description',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'init_robot_pose=fishbot_application.init_robot_pose:main',
        ],
    },
)

配置setup.py.注册节点，重新构建后先后运行gazebo模拟器,nav2

此时rviz处于左侧,启动后ros2 run fishbot_application init_robot_pose 效果

7.4.2 使用TF获取机器人的实时位置

新建get_robot_pose.py

import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion

class TFListener(Node):
    def __init__(self):
        super().__init__("tf2_listener")
        self.buffer_ = Buffer()
        self.listner_ = TransformListener(self.buffer_,self)
        self.timer_ =