【地平线旭日X3派试用体验】复现《人体跟随》小车（第五节）

        经过前面四节的准备工作，可以说万事具备，只欠东风，这个东风就是大功率充电宝，试了下自己的小容量的，带不起来，只好借了一个10000maH，5V/2A 输出的，X3派完美启动，好激动。

硬件准备好了，剩下的就就是程序了。主要参考了官方文章和论坛网友的例程，稍微修改了下。

官方介绍非常详细，点赞！

官方链接：人体跟随小车

一，准备工作。

见前面几节。

1. 旭日X3派已烧录好地平线提供的Ubuntu20.0.4或Linux系统镜像。

2. 旭日X3派已成功安装TogetherROS。

3. 旭日X3派已安装F37 sensor。

4. 和旭日X3派在同一网段（有线或者连接同一无线网，IP地址前三段需保持一致）的PC。

     5，小车硬件准备

二，启动官方例程。

使用MobaXterm，SSH登录，建立一个终端，输入以下指令

# 配置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  

三，启动小车接收node控制程序。

   上面的人体识别程序完全是官方的，没有改动。但小车控制程序，需要根据自己的小车进行修改，需要另外一个接收消息的程序。我的小车和官方的不一样，所以不能直接使用官方代码了，原理官方有介绍：

我的是X3派直接控制GPIO口，来控制驱动板，参考论坛里的一个网友的程序修改，下面是我的订阅/cmd_vel话题的控制消息 程序，

from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node

from std_msgs.msg import String

import Hobot.GPIO as GPIO
import time
import sys
import os
import serial
import serial.tools.list_ports

output_pin26 = 26
output_pin28 = 28
output_pin38 = 38 # BOARD 缂栫爜 38
output_pin40 = 40
GPIO.setmode(GPIO.BOARD)
GPIO.setup(output_pin26, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(output_pin28, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(output_pin38, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(output_pin40, GPIO.OUT, initial=GPIO.LOW)

uart_dev='/dev/ttyS3'
baudrate = 115200
try:
    ser = serial.Serial(uart_dev, int(baudrate), timeout=1) # 1s timeout
    print("open serial sucess")
except Exception as e:
    print("open serial failed!\n")

class CmdNode(Node):    
    def __init__(self,name):
        super().__init__(name)  
        self.sub_cmd = self.create_subscription(Twist,"cmd_vel",self.recv_cmd_callback,6)

    def recv_cmd_callback(self,cmd):
        self.cmd_speed=cmd._linear._x   #获取平移运动的步长
        self.cmd_angular=cmd._angular._z    #获取旋转运动的步长
       # print("get msg sucess")
        if(self.cmd_speed>0):            
            GPIO.output(output_pin26, GPIO.HIGH)
            GPIO.output(output_pin28, GPIO.LOW)
            GPIO.output(output_pin38, GPIO.HIGH)
            GPIO.output(output_pin40, GPIO.LOW)
            print("111")
        elif(self.cmd_angular>0): 
            GPIO.output(output_pin26, GPIO.LOW)
            GPIO.output(output_pin28, GPIO.LOW)
            GPIO.output(output_pin38, GPIO.HIGH)
            GPIO.output(output_pin40, GPIO.LOW)
            print("222")
        elif(self.cmd_angular<0): 
            GPIO.output(output_pin26, GPIO.HIGH)
            GPIO.output(output_pin28, GPIO.LOW)
            GPIO.output(output_pin38, GPIO.LOW)
            GPIO.output(output_pin40, GPIO.LOW)  
            print("333")       
        else:
            GPIO.output(output_pin26, GPIO.LOW)
            GPIO.output(output_pin28, GPIO.LOW)
            GPIO.output(output_pin38, GPIO.LOW)
            GPIO.output(output_pin40, GPIO.LOW) 
            print("444")

def main(args=None):
    rclpy.init(args=args) #初始化库
    sub_node = CmdNode("sub")  #新建节点对象
    rclpy.spin(sub_node) #spin循环节点
    rclpy.shutdown #关闭客户端

if __name__ == '__main__':
    main()

好了，准备好程序，另外新建一个终端，输入指令

# 配置TogetherROS环境 
source /opt/tros/setup.bash 

#启动订阅消息程序 
python3 /app/40pin_samples/zyd_ros_cmd_control.py 

end

四，效果展示视频