上一节我们是通过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_ = self.create_timer(1,self.get_transfrom)

    def get_transfrom(self):
        try:
            result = self.buffer_.lookup_transform('map','base_footprint',
                    rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))
            transform = result.transform
            self.get_logger().info(f'平移:{transform.translation}')
            self.get_logger().info(f'旋转:{transform.rotation}')
            ratation_euler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w]
            )
            self.get_logger().info(f'平移rpy:{ratation_euler}')
        except Exception as e:
            self.get_logger().warn(f'获取坐标转换异常{str(e)}')

def main():
    rclpy.init()
    node = TFListener()
    rclpy.spin(node)
    rclpy.shutdown()

代码跟之前5.2.3 一样。替换了下里面的坐标系，从map到base_footprint

setup.py注册节点,重新构建后运行模拟器、nav2,

运行：ros2 run fishbot_application get_robot_pose

开始机器人在原点附件，让机器人动起来。

[INFO] [1736928282.033835441] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=-6.652056569900022, y=-1.5508214853826923, z=0.0)
[INFO] [1736928282.035685589] [tf2_listener]: 旋转:geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.9995876581290986, w=0.02871434686675539)
[INFO] [1736928282.037711604] [tf2_listener]: 平移rpy:(0.0, -0.0, 3.084156065135798)
[INFO] [1736928283.037120375] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=-6.837668644286243, y=-1.5420274992480874, z=0.0)
[INFO] [1736928283.041984919] [tf2_listener]: 旋转:geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.9999243898774063, w=0.012296931661871013)
[INFO] [1736928283.044629005] [tf2_listener]: 平移rpy:(0.0, -0.0, 3.1169981703989635)
[INFO] [1736928284.032675742] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=-7.019270097528615, y=-1.5198555077016738, z=0.0)
[INFO] [1736928284.034688948] [tf2_listener]: 旋转:geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.9997611402893122, w=0.02185548826757744)
[INFO] [1736928284.036710963] [tf2_listener]: 平移rpy:(0.0, -0.0, 3.09787819645835)

7.4.3 调用接口进行单点导航

新建文件nav_to_pose.py

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

def main():
    rclpy.init()
    nav = BasicNavigator() #节点
    nav.waitUntilNav2Active() 

    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = nav.get_clock().now().to_msg()
    goal_pose.pose.position.x = 0.0
    goal_pose.pose.position.y = 0.0
    goal_pose.pose.orientation.w = 1.0
    nav.goToPose(goal_pose)
    while not nav.isTaskComplete():
        feedback = nav.getFeedback()
        nav.get_logger().info(f'剩余距离:{feedback.distance_remaining }')
    reslut = nav.getResult()
    nav.get_logger().info(f'导航结果:{reslut}')

启动仿真，启动nav2,启动nav_to_pose节点

[INFO] [1736938272.651787883] [basic_navigator]: 剩余距离:0.9807103872299194
[INFO] [1736938272.754783861] [basic_navigator]: 剩余距离:0.9557106494903564
[INFO] [1736938272.859448254] [basic_navigator]: 剩余距离:0.9307106137275696
[INFO] [1736938272.962284145] [basic_navigator]: 剩余距离:0.9057109355926514
[INFO] [1736938273.064598275] [basic_navigator]: 剩余距离:0.9057109355926514
[INFO] [1736938273.176734795] [basic_navigator]: 剩余距离:0.8663671016693115
[INFO] [1736938273.283178783] [basic_navigator]: 剩余距离:0.8663671016693115
[INFO] [1736938273.387921323] [basic_navigator]: 剩余距离:0.8307110071182251
[INFO] [1736938273.493608425] [basic_navigator]: 剩余距离:0.8057105541229248
[INFO] [1736938273.596150274] [basic_navigator]: 剩余距离:0.7807111144065857
[INFO] [1736938273.699119500] [basic_navigator]: 剩余距离:0.7307105660438538
[INFO] [1736938273.807311326] [basic_navigator]: 剩余距离:0.7057108879089355
[INFO] [1736938273.917285803] [basic_navigator]: 剩余距离:0.68071049451828
[INFO] [1736938274.019296920] [basic_navigator]: 剩余距离:0.6557106971740723
[INFO] [1736938274.124317370] [basic_navigator]: 剩余距离:0.6307108402252197
[INFO] [1736938274.229895621] [basic_navigator]: 剩余距离:0.630710244178772
[INFO] [1736938274.338002344] [basic_navigator]: 剩余距离:0.6057108044624329
[INFO] [1736938274.444569559] [basic_navigator]: 剩余距离:0.5807104110717773
[INFO] [1736938274.552454385] [basic_navigator]: 剩余距离:0.5557106733322144
[INFO] [1736938274.656308712] [basic_navigator]: 剩余距离:0.5557106733322144
[INFO] [1736938274.771069136] [basic_navigator]: 剩余距离:0.5307102799415588
[INFO] [1736938274.875142580] [basic_navigator]: 剩余距离:0.48071059584617615
[INFO] [1736938274.985280529] [basic_navigator]: 剩余距离:0.48071059584617615
[INFO] [1736938275.092809427] [basic_navigator]: 剩余距离:0.45571061968803406
[INFO] [1736938275.197257166] [basic_navigator]: 剩余距离:0.45281457901000977
[INFO] [1736938275.304928871] [basic_navigator]: 剩余距离:0.45281457901000977
[INFO] [1736938275.413466923] [basic_navigator]: 剩余距离:0.4057108163833618
[INFO] [1736938275.517761647] [basic_navigator]: 剩余距离:0.4057108163833618
[INFO] [1736938275.621373685] [basic_navigator]: 剩余距离:0.380710631608963
[INFO] [1736938275.725105424] [basic_navigator]: 剩余距离:0.380710631608963
[INFO] [1736938275.829369785] [basic_navigator]: 剩余距离:0.3557107448577881
[INFO] [1736938275.933044512] [basic_navigator]: 剩余距离:0.3307102620601654
[INFO] [1736938276.037629024] [basic_navigator]: 剩余距离:0.3307102620601654
[INFO] [1736938276.142200610] [basic_navigator]: 剩余距离:0.3307102620601654
[INFO] [1736938276.249326870] [basic_navigator]: 剩余距离:0.30571040511131287
[INFO] [1736938276.355518995] [basic_navigator]: 剩余距离:0.2807106077671051
[INFO] [1736938276.460481726] [basic_navigator]: 剩余距离:0.2807106077671051
[INFO] [1736938276.565703240] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938276.670670863] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938276.780405065] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938276.885576827] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938276.989968288] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938277.096139118] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938277.200539448] [basic_navigator]: 剩余距离:0.2557104825973511
[INFO] [1736938277.304825011] [basic_navigator]: 剩余距离:0.24885623157024384
[INFO] [1736938277.411476958] [basic_navigator]: 剩余距离:0.24885623157024384
[INFO] [1736938277.522958066] [basic_navigator]: 剩余距离:0.24885623157024384
[INFO] [1736938277.632039158] [basic_navigator]: 剩余距离:0.24885623157024384
[INFO] [1736938277.739100590] [basic_navigator]: 剩余距离:0.24885623157024384
[INFO] [1736938277.843913335] [basic_navigator]: 剩余距离:0.24885623157024384
[INFO] [1736938277.901751014] [basic_navigator]: 导航结果:TaskResult.SUCCEEDED

会看到机器人不断移动，到达目标点。

7.4.4 使用接口完成路点导航

新建文件：waypoint_followers.py

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

def main():
    rclpy.init()
    nav = BasicNavigator() #节点
    nav.waitUntilNav2Active() 
    goal_poses =[]

    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = nav.get_clock().now().to_msg()
    goal_pose.pose.position.x = 2.0
    goal_pose.pose.position.y = 1.0
    goal_pose.pose.orientation.w = 1.0
    goal_poses.append(goal_pose)
    goal_pose1 = PoseStamped()
    goal_pose1.header.frame_id = 'map'
    goal_pose1.header.stamp = nav.get_clock().now().to_msg()
    goal_pose1.pose.position.x = 0.0
    goal_pose1.pose.position.y = 1.0
    goal_pose1.pose.orientation.w = 1.0
    goal_poses.append(goal_pose1)

    goal_pose2 = PoseStamped()
    goal_pose2.header.frame_id = 'map'
    goal_pose2.header.stamp = nav.get_clock().now().to_msg()
    goal_pose2.pose.position.x = 0.0
    goal_pose2.pose.position.y = 0.0
    goal_pose2.pose.orientation.w = 1.0
    goal_poses.append(goal_pose2)


    nav.followWaypoints(goal_poses)
    while not nav.isTaskComplete():
        feedback = nav.getFeedback()
        nav.get_logger().info(f'剩余路点:{feedback.current_waypoint }')
    reslut = nav.getResult()
    nav.get_logger().info(f'导航结果:{reslut}')

 启动仿真，启动nav2,启动waypoint_follower节点

[INFO] [1736943164.759716760] [basic_navigator]: 剩余路点:2
[INFO] [1736943164.855499108] [basic_navigator]: 导航结果:TaskResult.SUCCEEDED

# 人工智能
Logo
乐奇 Rokid 开放社区

更多推荐

从对话到行动：用 AIUI 构建「喝水节奏教练」

「喝水节奏教练」是专为Rokid AI眼镜设计的本地化饮水记录智能体，通过自然语言指令实现“记一杯”“查进度”“撤销错误”等操作，数据全程本地存储。基于意图驱动的单页面UI，结合语音识别与A2UI动态渲染，支持快速记录、实时核对与修正，不依赖云端或健康数据，专注轻量可信的饮水节奏管理。

avatar 乐奇 Rokid 开放社区

智能眼镜市场全景解析：从技术演进到产业格局

本文系统梳理智能眼镜市场，涵盖产品形态、技术演进、市场规模、产业链结构及竞争格局。指出智能眼镜正从音频交互向AR显示与空间计算演进，受AI大模型、供应链成熟与生态延伸驱动，呈现分层发展态势。虽面临场景刚需不足、续航重量矛盾与隐私挑战，但B端应用先行、AI原生设计与标准完善将推动行业迈向规模化渗透，未来需关注核心技术突破与真实使用数据。

avatar 乐奇 Rokid 开放社区
cover

【日常逃跑计划：把世界戴在眼睛上，是什么体验？】

avatar 乐奇 Rokid 开放社区