开发者说 | 地平线程序员奶爸带你玩转机器人开发平台 —— 第二期 人体跟随
在ai操控机器人系列第一期的手势控制教程中，身为地平线资深程序员的奶爸，使用地平线发布的机器人开发平台togetherros软件栈，搭建了手势操控机器人。
当孩子的小伙伴们还是带个手套或者使用遥控器指挥机器人跑跑时，手势控制这么ai的机器人让小朋友得意了很长一段时间。（点击一下，内容回顾）
可是好景不长，小朋友对待玩具都是喜新厌旧的，这就给我提了新的需求，小小年纪就展现了当产品经理的潜力。
“能不能让机器人跟在我后面跑，就像咱们家狗狗那样呢？”
为了体现老爸的厉害，这个人体跟随机器人必须搞起（其实是机器人开发平台里面丰富的算法让我这么有底气）。
一、功能介绍
大家先看一下最终实现的功能。
从跟随效果来看，当人移动时，机器人能够迅速跟随人移动，得益于地平线旭日®️x3派（以下简称x3派）上5tops算力的bpu可以实现低延迟（30ms左右）、高帧率（满帧30fps）、远距离（>8m）、低系统负载（4核cpu只占用了单核8m）、低系统负载（单核cpu占用<40%）的算法推理效果，体现了x3 bpu强大的算力。
人体检测算法测试完成后，接着测试人体跟随功能。x3派上另起一个终端，执行如下命令查询x3派上的话题列表：
# 配置togetherros环境 source /opt/tros/setup.bash ros2 topic list
输出如下：
其中/cmd_vel话题是人体跟随控制节点发布的控制命令消息。在当前终端执行ros2 topic echo /cmd_vel命令查询x3派上的话题信息，当人体出现在f37摄像头前并且偏离正前方时，终端输出如下：
可以看到，此时人体跟随控制节点发布发布出的/cmd_vel话题的angular z数据为-0.5，表示以0.5r/s的速度转动。 说明人体识别算法和交互功能的软件包已安装成功，并能够通过/cmd_vel话题对外发布机器人运动控制消息。
将x3派安装到机器人上
将x3派安装到机器人上，并测试在x3派上通过发布/cmd_vel话题控制机器人运动的功能。连接了f37摄像头的x3派直接固定在机器人上，并将机器人的usb控制接口插到x3派上。 安装效果如下：
对于支持使用ros开发的机器人，一般会提供一个基于ros开发的机器人运动控制node，功能为订阅/cmd_vel话题的控制消息（ros2中定义的用于机器人控制的消息，消息类型为geometry_msgs/msg/twist），根据控制协议，通过usb等接口向机器人发送运动控制指令，实现控制机器人运动的目的。
本文使用的本末双足机器人使用的是usb接口，并提供了运行在x3派上的运动控制package，package订阅到/cmd_vel话题的控制消息后，通过usb向机器人下发控制指令，实现对机器人的控制。 在x3派上启动本末双足机器人运行控制node。打开一个终端，执行如下命令：
# 配置togetherros环境 source /opt/tros/setup.bash #启动本末机器人运动控制package ros2 run diablo_sdk ros_bridge_example
执行成功后终端中输出如下信息：
x3派上重新打开一个终端，通过发布/cmd_vel话题消息控制机器人以0.3r/s的速度转动：
# 配置togetherros环境 source /opt/tros/setup.bash ros2 topic pub -r 10 /cmd_vel geometry_msgs/twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.3}}'
执行成功后终端中输出如下信息：
机器人收到控制指令后转动的效果如下：
说明机器人能够按照发布的控制命令消息正确的实现运动。
对于其他机器人应该怎么安装
如果手里是其他的移动机器人，例如有一个使用树莓派或者jetson nano作为上位机的机器人，也可以将x3派安装在机器人上，代替树莓派或者jetson nano，实现控制机器人运动。 安装方法如下：
①编译可以运行在x3派上的运动控制package；
a.x3派上安装ros2软件系统构建和编译工具：
apt update apt-get install python3-catkin-pkg pip3 install empy pip3 install -u colcon-common-extensions
b.将原先运行在树莓派或者jetson nano上的机器人运动控制ros2 package源码拷贝到x3派上；
c.在x3派上，package源码工程所在路径下，直接使用source /opt/tros/setup.bash; colcon build命令编译package；
d.如果原先运动控制package是基于ros1开发，源码需要适配到ros2。只需要适配cmd_vel话题消息的订阅和处理，如果原先的ros1 package中有其他功能，可以先不关注。
②安装
a.将x3派固定在机器人上，如果空间有限，可以将原先的树莓派或者jetson nano拆除；
b.使用usb type c给x3派供电，如果机器人上无type c供电输出，也可以使用移动电源（输出至少5v&直流 2a）给x3派供电；
c.将机器人的usb控制接口插到x3派上。
③测试
a.x3派上启动新编译的机器人运动控制package；
b.x3派上重新打开一个终端，通过发布/cmd_vel话题消息控制机器人以0.3r/s的速度转动：
# 配置togetherros环境 source /opt/tros/setup.bash ros2 topic pub -r 10 /cmd_vel geometry_msgs/twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.3}}'
如果机器人正常转动，说明x3派安装成功。
四、完整的机器人人体跟随效果
下面开始测试完整的机器人人体跟随功能。
x3派上打开一个终端，启动人体检测和控制脚本
# 配置togetherros环境 source /opt/tros/setup.bash # 从togetherros的安装路径中拷贝出运行示例需要的配置文件。 cp -r /opt/tros/lib/mono2d_body_detection/config/ . #启动launch文件 ros2 launch body_tracking hobot_body_tracking_without_gesture.launch.py
x3派上打开一个终端，启动机器人运动控制node
# 配置togetherros环境 source /opt/tros/setup.bash #启动本末机器人运动控制node ros2 run diablo_sdk ros_bridge_example
机器人跟随人移动
五、原理分析
第4章节中，在x3派的两个终端中分别启动了hobot_body_tracking_without_gesture.launch.py脚本和运动控制node，实现了机器人跟随人体的效果，本章节对实现的原理进行分析。
app运行时的node和topic信息
对于一个基于ros开发的app，首先会想到这个app在运行时有哪些node，这些node发布和订阅了哪些topic，以及这些node之间的关联。
在x3派上使用ros2的命令行工具查询设备上运行的node和topic信息：
root@ubuntu:~# source /opt/tros/setup.bash root@ubuntu:~# ros2 node list /ai_msg_sub_node /hobot_codec1658324595406906403 /horizon_tracking_robotcmdvel /mipi_cam /mono2d_body_det /ros_bridge_example /tracking_strategy_parameter_node /transform_listener_impl_5594bf54a0 /websocket root@ubuntu:~# ros2 topic list /cmd_vel /hbmem_img080a1309022201080401012021072312 /hobot_mono2d_body_detection /image_jpeg /image_raw /imu/data_raw /odom /parameter_events /quat_odom /raw_odom /rosout /tf /tf_static
查询到x3派上运行着多个node，这些ros2 node之间是基于pub&sub机制通信，通过topic将这些node串联起来形成一个pipeline。
此app运行时node以及topic信息比较多，看不出node之间的关联。可以在pc端通过rqt（pc端需要安装ros2 foxy版本，rqt，以及pc需要和x3派处于同一网段）的node graph功能可以可视化的展示x3派上运行的node，node发布和订阅的topic，以及node之间的连接关系，如下图（其中椭圆形框内为node名，矩形框内为topic名）：
可以看到，整个graph（pipeline）以mipi_cam node（图像采集和发布）为起点，websocket node（序列化图片和ai结果，用于可视化展示）和ros_bridge_example node（机器人运动控制）为终点，起点和终点之间连接着多个node。这些node中ros_bridge_example node是通过ros2 run diablo_sdk ros_bridge_example命令启动，其余node都是通过hobot_body_tracking_without_gesture.launch.py脚本启动。
注意：/ai_msg_sub_node节点订阅到的/hobot_mono2d_body_detection话题消息实际是由/horizon_tracking_robotcmdvel节点处理，导致显示的graph不连续，属于设计上的待完善点
app的node介绍
对于复杂的包含多个node的应用，ros2提供了使用启动脚本通过launch批量启动node的功能。
机器人跟随人体app使用hobot_body_tracking_without_gesture.launch.py脚本来启动这些node，脚本内容如下：
import os from launch import launchdescription from launch_ros.actions import node from launch.actions import includelaunchdescription from launch.launch_description_sources import pythonlaunchdescriptionsource from ament_index_python import get_package_share_directory def generate_launch_description(): web_service_launch_include = includelaunchdescription( pythonlaunchdescriptionsource( os.path.join( get_package_share_directory('websocket'), 'launch/hobot_websocket_service.launch.py')) ) return launchdescription([ web_service_launch_include, # 启动图片发布pkg node( package='mipi_cam', executable='mipi_cam', output='screen', parameters=[ {out_format: nv12}, {image_width: 960}, {image_height: 544}, {io_method: shared_mem}, {video_device: f37} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动jpeg图片编码&发布pkg node( package='hobot_codec', executable='hobot_codec_republish', output='screen', parameters=[ {channel: 1}, {in_mode: shared_mem}, {in_format: nv12}, {out_mode: ros}, {out_format: jpeg}, {sub_topic: /hbmem_img}, {pub_topic: /image_jpeg} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动单目rgb人体、人头、人脸、人手框和人体关键点检测pkg node( package='mono2d_body_detection', executable='mono2d_body_detection', output='screen', parameters=[ {ai_msg_pub_topic_name: /hobot_mono2d_body_detection} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动web展示pkg node( package='websocket', executable='websocket', output='screen', parameters=[ {image_topic: /image_jpeg}, {image_type: mjpeg}, {smart_topic: /hobot_mono2d_body_detection} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动人体跟随pkg node( package='body_tracking', executable='body_tracking', output='screen', parameters=[ {ai_msg_sub_topic_name: /hobot_mono2d_body_detection}, {activate_wakeup_gesture: 0}, {img_width: 960}, {img_height: 544}, {track_serial_lost_num_thr: 30}, {move_step: 0.5}, {rotate_step: 0.5}, {activate_robot_move_thr: 5} ], arguments=['--ros-args', '--log-level', 'info'] ) ])
脚本中指定了多个node，其中每个node中的package配置项表示node名，可以看到脚本中配置的node名和5.1章节中查出来的一致。
在第1章节，分析了机器人人体跟随app所需要具备的功能模块，下面按照这些功能模块对node进行分析。
传感
使用了togetherros中的hobot sensor组件中的mipi_cam node，同时指定了node的参数：
out_format：发布图片的格式为nv12
image_width：图片分辨率宽为960
image_height：图片分辨率高为544
io_method：发布出来的图片传输方式为shared_mem，即共享内存方式，对应消息的topic为hbmem_img
video_device：指定了使用的mipi摄像头类型为f37，表示f37摄像头
参数指定了使用f37摄像头，发布的图片格式和分辨率可以直接用于算法推理，其中通过共享内存方式发布图片，可以极大地降低系统负载和传输延迟。
感知
使用了togetherros中的boxs算法仓库中的人体检测和跟踪算法，订阅hobot sensor（mipi_cam node）发布的图像消息，利用bpu处理器进行ai推理，发布包含人体、人头、人脸、人手框和人体骨骼关键点检测结果的ai msg，并通过多目标跟踪（multi-target tracking，即mot）功能，实现检测框的跟踪和目标编号分配。
node对应package名为'mono2d_body_detection'，同时指定了node的参数：
ai_msg_pub_topic_name：发布包含人体检测框信息的ai感知结果的topic名为/hobot_mono2d_body_detection
人体检测和跟踪算法推理node（mono2dbodydetnode）主要包含三部分逻辑独立的功能：
node初始化和启动
配置算法使用的模型信息，创建消息的发布者和订阅者，启动目标跟踪算法引擎；
订阅消息和算法推理
在注册的图像消息回调中，处理图像数据并用于算法模型推理，回调中不等待算法推理完成；
推理结果的处理和发布
算法推理完成后，通过注册的回调postprocess输出推理结果，回调中对检测结果按照时间顺序排序（nodeoutputmanage）以及进行多目标跟踪算法（hobotmot）处理后，发布算法推理结果消息。
node的设计和流程逻辑如下图：
交互
人体跟随策略node订阅人体检测和跟踪算法发布的包含人体信息的ai msg，根据人体框和机器人的位置关系，发布前进、后退、左转、右转的控制消息，实现控制机器人运动。 node发布的运动控制消息为ros2中定义的消息，topic为“/cmd_vel”，消息类型为“geometry_msgs/msg/twist”，node对应package名为'body_tracking'，同时指定了node的参数：
ai_msg_sub_topic_name: 订阅包含人体检测信息的topic名为/hobot_mono2d_body_detection
activate_wakeup_gesture: 唤醒手势开关，值为0表示不启用唤醒手势。一般在人较多，环境复杂的场景，通过启用唤醒手势避免误触发人体跟随功能。
img_width: 人体检测模型输入图片像素的宽
img_height: 人体检测模型输入图片像素的高
track_serial_lost_num_thr: 人体连续消失帧数阈值，值为30，表示当跟随的人体连续消失30帧之后会重新选择跟随的人体
move_step: 平移运动的步长（速度），0.5表示移动速度为0.5m/s，值越大速度越快
rotate_step: 旋转运动的步长（速度），0.5表示旋转速度为0.5r/s，值越大速度越快
activate_robot_move_thr: 激活机器人移动的阈值，单位为像素。当人体检测框的y像素值小于阈值（具体画面的上边界）时，激活机器人移动。
人体跟随策略选择第一次出现的人体作为跟随人体，如果有多个人体同时出现，选择最大宽度的人体检测框作为跟随人体；已有跟随人的情况下，其他的人体检测结果都无效。 node输出的log中tracking_sta关键字表示跟随的状态，0表示未找到跟随人，1表示已有跟随人，2表示跟随人消失。node启动后，未找到跟随人的情况下，输出log中tracking_sta值为0；当有跟随人时，输出log中tracking_sta值为1，同时通过track_id关键字输出跟随人的编号；只有当跟随人消失，即连续track_serial_lost_num_thr帧（配置为30帧，对于输出频率为30fps的f37摄像头，为1秒）未检测到跟随人，判断跟随人消失，开始重新选择跟随人，跟随人消失时输出log中tracking_sta值为2。
如果需要启用唤醒手势避免误触发，唤醒手势使用方法详见人体跟随策略的代码仓库。
控制
机器人运动控制node订阅人体跟随策略node发布的topic为“/cmd_vel”的控制消息，根据控制协议，通过usb总线向机器人下位机发布运动控制指令。对于不同类型的机器人，控制协议不同，对应于不同的运动控制node。本文使用的是本末双足机器人，对应的运动控制node启动方法为ros2 run diablo_sdk ros_bridge_example。此node单独启动，不在启动脚本中。
app的系统设计
根据5.1和5.2章节的介绍，已经知道了机器人人体跟随app启动了哪些node，这些node的功能，node之间的关系，以及使用这些node如何实现通过人体跟随机器人的目标。下面进行理论总结，介绍此app的系统设计。对于一个复杂的机器人系统，一般在机器人上配置上位机和下位机两种处理器，机器人上位机的计算能力较强，执行复杂的机器人上层应用，同时能够最大程度屏蔽不同类型机器人的底层差异；机器人下位机一般使用低成本的mcu处理器，对机器人本体上的各类传感器和硬件进行数据采集/控制。
人体跟随app由两部分组成，分别为机器人和pc端，其中机器人部分又分为上位机和下位机。详细组成如下图：
机器人上位机为x3派，运行着多个ros2 node，除了5.2章节介绍的传感、感知、交互和控制功能，还有jpeg图像编码和web展示功能，将摄像头发布的图片编码压缩，以及将人体检测和跟踪算法发布的ai数据序列化后使用websocket协议发布，实现跨设备在pc端渲染展示和调试（机器人下位机属于机器人本体的一部分，详细说明略）。
从app的系统设计图中可以看出，搭载了x3派和togetherros的机器人，利用芯片的ai加速能力和togetherros中丰富的算法、机器人开发组件，可以实现快速开发智能机器人应用的目标。
六、faq
如何复现app效果？
复现app效果涉及到两部分：
（1）机器人人体跟随app
参考第2章准备工作，在x3派上安装togetherros。
（2）本末双足机器人和机器人运动控制package
获取方法详见产品信息：https://developer.horizon.ai/forumdetail/94246984227025410
除了本末双足机器人，x3派和togetherros还适配了小r科技的麦轮小车，也可以使用小r小车直接体验app效果。
没有机器人的情况下可以体验app效果吗？
可以体验。 在没有机器人的情况下，可以使用此app控制gazebo仿真环境下的虚拟机器人运动。
如何将app适配到自己的机器人上？
本文以本末双足机器人为例介绍人体跟随app的效果，app本身不依赖于任何形态的机器人，app发布的运动控制消息为ros2中定义的消息（topic为“/cmd_vel”，消息类型为“geometry_msgs/msg/twist”，具体说明参考第5章的原理分析）。
如下图，可以将app的组成划分成红色和蓝色虚线框两个部分：
（1）红色虚线框部分
这部分功能不依赖于机器人，即可以直接移植到任意形态的机器人上。
移植方法为将x3派安装在机器人上，按照第2章节的准备工作，在x3派上安装摄像头传感器和togetherros。
（2）蓝色虚线框部分
这部分功能依赖于机器人，需要针对性的适配。根据机器人的状态不同，对应不同的适配方法。
状态1，原先机器人上有上位机和下位机，如原先使用树莓派或者jetson nano作为上位机，并且上位机上有机器人运动控制node。需要在x3派上重新编译机器人运动控制node。
状态2，原先机器人上只有下位机。需要开发机器人运动控制node后（开发参考components/xrrobot · develop · hhp / app / xr_robot · gitlab (horizon.ai)），在x3派上编译机器人运动控制node。
app支持哪些摄像头？
app对于摄像头类型没有要求，地平线机器人平台支持mipi和usb两类摄像头。对于mipi摄像头，支持f37和gc4663两种型号。
如何调整机器人的运动速度？
修改app启动脚本hobot_body_tracking_without_gesture.launch.py中人体跟随策略body_tracking node中的move_step和rotate_step参数，可以控制机器人的平移和旋转速度：
# 启动人体跟随pkg node( package='body_tracking', executable='body_tracking', output='screen', parameters=[ {ai_msg_sub_topic_name: /hobot_mono2d_body_detection}, {activate_wakeup_gesture: 0}, {img_width: 960}, {img_height: 544}, {track_serial_lost_num_thr: 30}, {move_step: 0.5}, {rotate_step: 0.5}, {activate_robot_move_thr: 5} ], arguments=['--ros-args', '--log-level', 'info'] )
可以开发一个python的node扩展app功能吗？
可以，ros2支持跨设备、跨平台、跨语言，togetherros完全兼容ros2 foxy版本，因此也支持这些特性。
例如根据检出的人体骨骼关键点，开发交互node，控制人形机器人模仿人的肢体动作。订阅hobot_mono2d_body_detection话题消息（包含人体骨骼关键点信息），使用命令可以查询到话题的详细信息：
# ros2 topic info /hobot_mono2d_body_detection type: ai_msgs/msg/perceptiontargets publisher count: 1 subscription count: 1
消息类型为ai_msgs/msg/perceptiontargets，因此需要从ai_msgs.msg中import perceptiontargets消息package。
完整的python代码示例：
import rclpy from rclpy.node import node from ai_msgs.msg import perceptiontargets class minimalsubscriber(node): def __init__(self): super().__init__('minimal_subscriber') self.subscription = self.create_subscription( perceptiontargets, 'hobot_mono2d_body_detection', self.listener_callback, 10) self.subscription # prevent unused variable warning def listener_callback(self, msg): self.get_logger().info('i heard: %s\n' % msg) def main(args=none): rclpy.init(args=args) minimal_subscriber = minimalsubscriber() rclpy.spin(minimal_subscriber) # destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) minimal_subscriber.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
使用python开发完node后，直接在x3派上编译并运行。运行并输出订阅到的消息：
如何开发一个自己的算法node扩展app功能？
togetherros提供的hobot dnn简化板端ai模型推理与部署，释放bpu算力，降低ai使用门槛。同时内置了常用的检测、分类和分割算法的模型后处理，帮助用户快速在x3派上集成部署自己的算法。
本文转自地平线开发者社区
原作者：zhuk
原链接：https://developer.horizon.ai/forumdetail/106482197149630465
欧司朗新发布的能使LED更高效新型光转换技术资料说明
接触器和继电器有何区别
云耀云服务器 L 实例：轻应用部署的理想选择，高性价比无忧使用
国产车用芯片FM33LG0xxA开发板,他来啦!
基于ARM芯片实现旋转热管多点温度遥测系统的设计
开发者说 | 地平线程序员奶爸带你玩转机器人开发平台 —— 第二期 人体跟随
全民居家办公时代必备：aigo国民好物移动固态硬盘
创维电视好吗？智能悦见世界
与柴火饭媲美！EN单片机芯片方案开发的智能电饭煲做到了
智慧物联网是如何去定义的
垂直轴小型风力发电机与水平轴小型风力发电机对比
自制pcb需要注意哪一些点
S7-300 PLC在变电站中的应用
Intel下一代核心i3 CPU将在台积电5nm工艺上生产
Problems and Solutions for Adj
2018年深圳国际电路板采购展即将开始,CS show 2018同期论坛四大看点
创新型焊接桥式整流器，提高电子产品生产工艺！
了解RISC OS桌面开源操作系统的基础知识
数字资产量化交易系统开发自动交易软件开发
逆变电源效率作用，应该考虑负载
当孩子的小伙伴们还是带个手套或者使用遥控器指挥机器人跑跑时，手势控制这么ai的机器人让小朋友得意了很长一段时间。（点击一下，内容回顾）
可是好景不长，小朋友对待玩具都是喜新厌旧的，这就给我提了新的需求，小小年纪就展现了当产品经理的潜力。
“能不能让机器人跟在我后面跑，就像咱们家狗狗那样呢？”
为了体现老爸的厉害，这个人体跟随机器人必须搞起（其实是机器人开发平台里面丰富的算法让我这么有底气）。
一、功能介绍
大家先看一下最终实现的功能。
从跟随效果来看，当人移动时，机器人能够迅速跟随人移动，得益于地平线旭日®️x3派（以下简称x3派）上5tops算力的bpu可以实现低延迟（30ms左右）、高帧率（满帧30fps）、远距离（>8m）、低系统负载（4核cpu只占用了单核8m）、低系统负载（单核cpu占用<40%）的算法推理效果，体现了x3 bpu强大的算力。
人体检测算法测试完成后，接着测试人体跟随功能。x3派上另起一个终端，执行如下命令查询x3派上的话题列表：
# 配置togetherros环境 source /opt/tros/setup.bash ros2 topic list
输出如下：
其中/cmd_vel话题是人体跟随控制节点发布的控制命令消息。在当前终端执行ros2 topic echo /cmd_vel命令查询x3派上的话题信息，当人体出现在f37摄像头前并且偏离正前方时，终端输出如下：
可以看到，此时人体跟随控制节点发布发布出的/cmd_vel话题的angular z数据为-0.5，表示以0.5r/s的速度转动。 说明人体识别算法和交互功能的软件包已安装成功，并能够通过/cmd_vel话题对外发布机器人运动控制消息。
将x3派安装到机器人上
将x3派安装到机器人上，并测试在x3派上通过发布/cmd_vel话题控制机器人运动的功能。连接了f37摄像头的x3派直接固定在机器人上，并将机器人的usb控制接口插到x3派上。 安装效果如下：
对于支持使用ros开发的机器人，一般会提供一个基于ros开发的机器人运动控制node，功能为订阅/cmd_vel话题的控制消息（ros2中定义的用于机器人控制的消息，消息类型为geometry_msgs/msg/twist），根据控制协议，通过usb等接口向机器人发送运动控制指令，实现控制机器人运动的目的。
本文使用的本末双足机器人使用的是usb接口，并提供了运行在x3派上的运动控制package，package订阅到/cmd_vel话题的控制消息后，通过usb向机器人下发控制指令，实现对机器人的控制。 在x3派上启动本末双足机器人运行控制node。打开一个终端，执行如下命令：
# 配置togetherros环境 source /opt/tros/setup.bash #启动本末机器人运动控制package ros2 run diablo_sdk ros_bridge_example
执行成功后终端中输出如下信息：
x3派上重新打开一个终端，通过发布/cmd_vel话题消息控制机器人以0.3r/s的速度转动：
# 配置togetherros环境 source /opt/tros/setup.bash ros2 topic pub -r 10 /cmd_vel geometry_msgs/twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.3}}'
执行成功后终端中输出如下信息：
机器人收到控制指令后转动的效果如下：
说明机器人能够按照发布的控制命令消息正确的实现运动。
对于其他机器人应该怎么安装
如果手里是其他的移动机器人，例如有一个使用树莓派或者jetson nano作为上位机的机器人，也可以将x3派安装在机器人上，代替树莓派或者jetson nano，实现控制机器人运动。 安装方法如下：
①编译可以运行在x3派上的运动控制package；
a.x3派上安装ros2软件系统构建和编译工具：
apt update apt-get install python3-catkin-pkg pip3 install empy pip3 install -u colcon-common-extensions
b.将原先运行在树莓派或者jetson nano上的机器人运动控制ros2 package源码拷贝到x3派上；
c.在x3派上，package源码工程所在路径下，直接使用source /opt/tros/setup.bash; colcon build命令编译package；
d.如果原先运动控制package是基于ros1开发，源码需要适配到ros2。只需要适配cmd_vel话题消息的订阅和处理，如果原先的ros1 package中有其他功能，可以先不关注。
②安装
a.将x3派固定在机器人上，如果空间有限，可以将原先的树莓派或者jetson nano拆除；
b.使用usb type c给x3派供电，如果机器人上无type c供电输出，也可以使用移动电源（输出至少5v&直流 2a）给x3派供电；
c.将机器人的usb控制接口插到x3派上。
③测试
a.x3派上启动新编译的机器人运动控制package；
b.x3派上重新打开一个终端，通过发布/cmd_vel话题消息控制机器人以0.3r/s的速度转动：
# 配置togetherros环境 source /opt/tros/setup.bash ros2 topic pub -r 10 /cmd_vel geometry_msgs/twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.3}}'
如果机器人正常转动，说明x3派安装成功。
四、完整的机器人人体跟随效果
下面开始测试完整的机器人人体跟随功能。
x3派上打开一个终端，启动人体检测和控制脚本
# 配置togetherros环境 source /opt/tros/setup.bash # 从togetherros的安装路径中拷贝出运行示例需要的配置文件。 cp -r /opt/tros/lib/mono2d_body_detection/config/ . #启动launch文件 ros2 launch body_tracking hobot_body_tracking_without_gesture.launch.py
x3派上打开一个终端，启动机器人运动控制node
# 配置togetherros环境 source /opt/tros/setup.bash #启动本末机器人运动控制node ros2 run diablo_sdk ros_bridge_example
机器人跟随人移动
五、原理分析
第4章节中，在x3派的两个终端中分别启动了hobot_body_tracking_without_gesture.launch.py脚本和运动控制node，实现了机器人跟随人体的效果，本章节对实现的原理进行分析。
app运行时的node和topic信息
对于一个基于ros开发的app，首先会想到这个app在运行时有哪些node，这些node发布和订阅了哪些topic，以及这些node之间的关联。
在x3派上使用ros2的命令行工具查询设备上运行的node和topic信息：
root@ubuntu:~# source /opt/tros/setup.bash root@ubuntu:~# ros2 node list /ai_msg_sub_node /hobot_codec1658324595406906403 /horizon_tracking_robotcmdvel /mipi_cam /mono2d_body_det /ros_bridge_example /tracking_strategy_parameter_node /transform_listener_impl_5594bf54a0 /websocket root@ubuntu:~# ros2 topic list /cmd_vel /hbmem_img080a1309022201080401012021072312 /hobot_mono2d_body_detection /image_jpeg /image_raw /imu/data_raw /odom /parameter_events /quat_odom /raw_odom /rosout /tf /tf_static
查询到x3派上运行着多个node，这些ros2 node之间是基于pub&sub机制通信，通过topic将这些node串联起来形成一个pipeline。
此app运行时node以及topic信息比较多，看不出node之间的关联。可以在pc端通过rqt（pc端需要安装ros2 foxy版本，rqt，以及pc需要和x3派处于同一网段）的node graph功能可以可视化的展示x3派上运行的node，node发布和订阅的topic，以及node之间的连接关系，如下图（其中椭圆形框内为node名，矩形框内为topic名）：
可以看到，整个graph（pipeline）以mipi_cam node（图像采集和发布）为起点，websocket node（序列化图片和ai结果，用于可视化展示）和ros_bridge_example node（机器人运动控制）为终点，起点和终点之间连接着多个node。这些node中ros_bridge_example node是通过ros2 run diablo_sdk ros_bridge_example命令启动，其余node都是通过hobot_body_tracking_without_gesture.launch.py脚本启动。
注意：/ai_msg_sub_node节点订阅到的/hobot_mono2d_body_detection话题消息实际是由/horizon_tracking_robotcmdvel节点处理，导致显示的graph不连续，属于设计上的待完善点
app的node介绍
对于复杂的包含多个node的应用，ros2提供了使用启动脚本通过launch批量启动node的功能。
机器人跟随人体app使用hobot_body_tracking_without_gesture.launch.py脚本来启动这些node，脚本内容如下：
import os from launch import launchdescription from launch_ros.actions import node from launch.actions import includelaunchdescription from launch.launch_description_sources import pythonlaunchdescriptionsource from ament_index_python import get_package_share_directory def generate_launch_description(): web_service_launch_include = includelaunchdescription( pythonlaunchdescriptionsource( os.path.join( get_package_share_directory('websocket'), 'launch/hobot_websocket_service.launch.py')) ) return launchdescription([ web_service_launch_include, # 启动图片发布pkg node( package='mipi_cam', executable='mipi_cam', output='screen', parameters=[ {out_format: nv12}, {image_width: 960}, {image_height: 544}, {io_method: shared_mem}, {video_device: f37} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动jpeg图片编码&发布pkg node( package='hobot_codec', executable='hobot_codec_republish', output='screen', parameters=[ {channel: 1}, {in_mode: shared_mem}, {in_format: nv12}, {out_mode: ros}, {out_format: jpeg}, {sub_topic: /hbmem_img}, {pub_topic: /image_jpeg} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动单目rgb人体、人头、人脸、人手框和人体关键点检测pkg node( package='mono2d_body_detection', executable='mono2d_body_detection', output='screen', parameters=[ {ai_msg_pub_topic_name: /hobot_mono2d_body_detection} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动web展示pkg node( package='websocket', executable='websocket', output='screen', parameters=[ {image_topic: /image_jpeg}, {image_type: mjpeg}, {smart_topic: /hobot_mono2d_body_detection} ], arguments=['--ros-args', '--log-level', 'error'] ), # 启动人体跟随pkg node( package='body_tracking', executable='body_tracking', output='screen', parameters=[ {ai_msg_sub_topic_name: /hobot_mono2d_body_detection}, {activate_wakeup_gesture: 0}, {img_width: 960}, {img_height: 544}, {track_serial_lost_num_thr: 30}, {move_step: 0.5}, {rotate_step: 0.5}, {activate_robot_move_thr: 5} ], arguments=['--ros-args', '--log-level', 'info'] ) ])
脚本中指定了多个node，其中每个node中的package配置项表示node名，可以看到脚本中配置的node名和5.1章节中查出来的一致。
在第1章节，分析了机器人人体跟随app所需要具备的功能模块，下面按照这些功能模块对node进行分析。
传感
使用了togetherros中的hobot sensor组件中的mipi_cam node，同时指定了node的参数：
out_format：发布图片的格式为nv12
image_width：图片分辨率宽为960
image_height：图片分辨率高为544
io_method：发布出来的图片传输方式为shared_mem，即共享内存方式，对应消息的topic为hbmem_img
video_device：指定了使用的mipi摄像头类型为f37，表示f37摄像头
参数指定了使用f37摄像头，发布的图片格式和分辨率可以直接用于算法推理，其中通过共享内存方式发布图片，可以极大地降低系统负载和传输延迟。
感知
使用了togetherros中的boxs算法仓库中的人体检测和跟踪算法，订阅hobot sensor（mipi_cam node）发布的图像消息，利用bpu处理器进行ai推理，发布包含人体、人头、人脸、人手框和人体骨骼关键点检测结果的ai msg，并通过多目标跟踪（multi-target tracking，即mot）功能，实现检测框的跟踪和目标编号分配。
node对应package名为'mono2d_body_detection'，同时指定了node的参数：
ai_msg_pub_topic_name：发布包含人体检测框信息的ai感知结果的topic名为/hobot_mono2d_body_detection
人体检测和跟踪算法推理node（mono2dbodydetnode）主要包含三部分逻辑独立的功能：
node初始化和启动
配置算法使用的模型信息，创建消息的发布者和订阅者，启动目标跟踪算法引擎；
订阅消息和算法推理
在注册的图像消息回调中，处理图像数据并用于算法模型推理，回调中不等待算法推理完成；
推理结果的处理和发布
算法推理完成后，通过注册的回调postprocess输出推理结果，回调中对检测结果按照时间顺序排序（nodeoutputmanage）以及进行多目标跟踪算法（hobotmot）处理后，发布算法推理结果消息。
node的设计和流程逻辑如下图：
交互
人体跟随策略node订阅人体检测和跟踪算法发布的包含人体信息的ai msg，根据人体框和机器人的位置关系，发布前进、后退、左转、右转的控制消息，实现控制机器人运动。 node发布的运动控制消息为ros2中定义的消息，topic为“/cmd_vel”，消息类型为“geometry_msgs/msg/twist”，node对应package名为'body_tracking'，同时指定了node的参数：
ai_msg_sub_topic_name: 订阅包含人体检测信息的topic名为/hobot_mono2d_body_detection
activate_wakeup_gesture: 唤醒手势开关，值为0表示不启用唤醒手势。一般在人较多，环境复杂的场景，通过启用唤醒手势避免误触发人体跟随功能。
img_width: 人体检测模型输入图片像素的宽
img_height: 人体检测模型输入图片像素的高
track_serial_lost_num_thr: 人体连续消失帧数阈值，值为30，表示当跟随的人体连续消失30帧之后会重新选择跟随的人体
move_step: 平移运动的步长（速度），0.5表示移动速度为0.5m/s，值越大速度越快
rotate_step: 旋转运动的步长（速度），0.5表示旋转速度为0.5r/s，值越大速度越快
activate_robot_move_thr: 激活机器人移动的阈值，单位为像素。当人体检测框的y像素值小于阈值（具体画面的上边界）时，激活机器人移动。
人体跟随策略选择第一次出现的人体作为跟随人体，如果有多个人体同时出现，选择最大宽度的人体检测框作为跟随人体；已有跟随人的情况下，其他的人体检测结果都无效。 node输出的log中tracking_sta关键字表示跟随的状态，0表示未找到跟随人，1表示已有跟随人，2表示跟随人消失。node启动后，未找到跟随人的情况下，输出log中tracking_sta值为0；当有跟随人时，输出log中tracking_sta值为1，同时通过track_id关键字输出跟随人的编号；只有当跟随人消失，即连续track_serial_lost_num_thr帧（配置为30帧，对于输出频率为30fps的f37摄像头，为1秒）未检测到跟随人，判断跟随人消失，开始重新选择跟随人，跟随人消失时输出log中tracking_sta值为2。
如果需要启用唤醒手势避免误触发，唤醒手势使用方法详见人体跟随策略的代码仓库。
控制
机器人运动控制node订阅人体跟随策略node发布的topic为“/cmd_vel”的控制消息，根据控制协议，通过usb总线向机器人下位机发布运动控制指令。对于不同类型的机器人，控制协议不同，对应于不同的运动控制node。本文使用的是本末双足机器人，对应的运动控制node启动方法为ros2 run diablo_sdk ros_bridge_example。此node单独启动，不在启动脚本中。
app的系统设计
根据5.1和5.2章节的介绍，已经知道了机器人人体跟随app启动了哪些node，这些node的功能，node之间的关系，以及使用这些node如何实现通过人体跟随机器人的目标。下面进行理论总结，介绍此app的系统设计。对于一个复杂的机器人系统，一般在机器人上配置上位机和下位机两种处理器，机器人上位机的计算能力较强，执行复杂的机器人上层应用，同时能够最大程度屏蔽不同类型机器人的底层差异；机器人下位机一般使用低成本的mcu处理器，对机器人本体上的各类传感器和硬件进行数据采集/控制。
人体跟随app由两部分组成，分别为机器人和pc端，其中机器人部分又分为上位机和下位机。详细组成如下图：
机器人上位机为x3派，运行着多个ros2 node，除了5.2章节介绍的传感、感知、交互和控制功能，还有jpeg图像编码和web展示功能，将摄像头发布的图片编码压缩，以及将人体检测和跟踪算法发布的ai数据序列化后使用websocket协议发布，实现跨设备在pc端渲染展示和调试（机器人下位机属于机器人本体的一部分，详细说明略）。
从app的系统设计图中可以看出，搭载了x3派和togetherros的机器人，利用芯片的ai加速能力和togetherros中丰富的算法、机器人开发组件，可以实现快速开发智能机器人应用的目标。
六、faq
如何复现app效果？
复现app效果涉及到两部分：
（1）机器人人体跟随app
参考第2章准备工作，在x3派上安装togetherros。
（2）本末双足机器人和机器人运动控制package
获取方法详见产品信息：https://developer.horizon.ai/forumdetail/94246984227025410
除了本末双足机器人，x3派和togetherros还适配了小r科技的麦轮小车，也可以使用小r小车直接体验app效果。
没有机器人的情况下可以体验app效果吗？
可以体验。 在没有机器人的情况下，可以使用此app控制gazebo仿真环境下的虚拟机器人运动。
如何将app适配到自己的机器人上？
本文以本末双足机器人为例介绍人体跟随app的效果，app本身不依赖于任何形态的机器人，app发布的运动控制消息为ros2中定义的消息（topic为“/cmd_vel”，消息类型为“geometry_msgs/msg/twist”，具体说明参考第5章的原理分析）。
如下图，可以将app的组成划分成红色和蓝色虚线框两个部分：
（1）红色虚线框部分
这部分功能不依赖于机器人，即可以直接移植到任意形态的机器人上。
移植方法为将x3派安装在机器人上，按照第2章节的准备工作，在x3派上安装摄像头传感器和togetherros。
（2）蓝色虚线框部分
这部分功能依赖于机器人，需要针对性的适配。根据机器人的状态不同，对应不同的适配方法。
状态1，原先机器人上有上位机和下位机，如原先使用树莓派或者jetson nano作为上位机，并且上位机上有机器人运动控制node。需要在x3派上重新编译机器人运动控制node。
状态2，原先机器人上只有下位机。需要开发机器人运动控制node后（开发参考components/xrrobot · develop · hhp / app / xr_robot · gitlab (horizon.ai)），在x3派上编译机器人运动控制node。
app支持哪些摄像头？
app对于摄像头类型没有要求，地平线机器人平台支持mipi和usb两类摄像头。对于mipi摄像头，支持f37和gc4663两种型号。
如何调整机器人的运动速度？
修改app启动脚本hobot_body_tracking_without_gesture.launch.py中人体跟随策略body_tracking node中的move_step和rotate_step参数，可以控制机器人的平移和旋转速度：
# 启动人体跟随pkg node( package='body_tracking', executable='body_tracking', output='screen', parameters=[ {ai_msg_sub_topic_name: /hobot_mono2d_body_detection}, {activate_wakeup_gesture: 0}, {img_width: 960}, {img_height: 544}, {track_serial_lost_num_thr: 30}, {move_step: 0.5}, {rotate_step: 0.5}, {activate_robot_move_thr: 5} ], arguments=['--ros-args', '--log-level', 'info'] )
可以开发一个python的node扩展app功能吗？
可以，ros2支持跨设备、跨平台、跨语言，togetherros完全兼容ros2 foxy版本，因此也支持这些特性。
例如根据检出的人体骨骼关键点，开发交互node，控制人形机器人模仿人的肢体动作。订阅hobot_mono2d_body_detection话题消息（包含人体骨骼关键点信息），使用命令可以查询到话题的详细信息：
# ros2 topic info /hobot_mono2d_body_detection type: ai_msgs/msg/perceptiontargets publisher count: 1 subscription count: 1
消息类型为ai_msgs/msg/perceptiontargets，因此需要从ai_msgs.msg中import perceptiontargets消息package。
完整的python代码示例：
import rclpy from rclpy.node import node from ai_msgs.msg import perceptiontargets class minimalsubscriber(node): def __init__(self): super().__init__('minimal_subscriber') self.subscription = self.create_subscription( perceptiontargets, 'hobot_mono2d_body_detection', self.listener_callback, 10) self.subscription # prevent unused variable warning def listener_callback(self, msg): self.get_logger().info('i heard: %s\n' % msg) def main(args=none): rclpy.init(args=args) minimal_subscriber = minimalsubscriber() rclpy.spin(minimal_subscriber) # destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) minimal_subscriber.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
使用python开发完node后，直接在x3派上编译并运行。运行并输出订阅到的消息：
如何开发一个自己的算法node扩展app功能？
togetherros提供的hobot dnn简化板端ai模型推理与部署，释放bpu算力，降低ai使用门槛。同时内置了常用的检测、分类和分割算法的模型后处理，帮助用户快速在x3派上集成部署自己的算法。
本文转自地平线开发者社区
原作者：zhuk
原链接：https://developer.horizon.ai/forumdetail/106482197149630465
欧司朗新发布的能使LED更高效新型光转换技术资料说明
接触器和继电器有何区别
云耀云服务器 L 实例：轻应用部署的理想选择，高性价比无忧使用
国产车用芯片FM33LG0xxA开发板,他来啦!
基于ARM芯片实现旋转热管多点温度遥测系统的设计
开发者说 | 地平线程序员奶爸带你玩转机器人开发平台 —— 第二期 人体跟随
全民居家办公时代必备：aigo国民好物移动固态硬盘
创维电视好吗？智能悦见世界
与柴火饭媲美！EN单片机芯片方案开发的智能电饭煲做到了
智慧物联网是如何去定义的
垂直轴小型风力发电机与水平轴小型风力发电机对比
自制pcb需要注意哪一些点
S7-300 PLC在变电站中的应用
Intel下一代核心i3 CPU将在台积电5nm工艺上生产
Problems and Solutions for Adj
2018年深圳国际电路板采购展即将开始,CS show 2018同期论坛四大看点
创新型焊接桥式整流器，提高电子产品生产工艺！
了解RISC OS桌面开源操作系统的基础知识
数字资产量化交易系统开发自动交易软件开发
逆变电源效率作用，应该考虑负载