机器人手柄控制

1、程序功能说明

小车连接上代理，把手柄的接收器接入树莓派5端口，运行程序，即可用遥控控制小车运动、蜂鸣器和云台舵机。

2、启动并连接代理

RISC-V小车成功开机后，需要临时关闭掉自启动程序防止冲突（详情查看《0.开机自启动和临时关闭》），打开终端输入以下指令打开代理，连接成功如下图所示，

sh ~/sh start_agent.sh

注：如果卡在前面两条log，可以按一下小车上的复位按键，这是因为第二次连代理需要重置一下。

3、启动手柄控制程序

手动打开手柄控制程序。打开一个终端输入

xxxxxxxxxx
#手柄遥控控制小车程序
ros2 run joy joy_node

打开另一个终端输入

xxxxxxxxxx
ros2 run yahboomcar_ctrl yahboom_joy

遥控按键说明如下：

• 左边摇杆：前后方向有效，控制小车前进后退，左右方向无效

• 右边摇杆：左右方向有效，控制小车左右转动，前后方向无效

• START键：蜂鸣器控制

• Y键：控制S2舵机向上

• A键：控制S2舵机向下

• X键：控制S1舵机向左

• B键：控制S1舵机向右

• R1键：手柄控制速度开关，按下后才能用遥控控制小车速度，再次按下则失去手柄控制速度，同时 也是玩法开关，按下后停车，再次按下则是继续运行功能玩法程序，包括雷达避障、雷达警卫等 等。

• MODE键：切换模式，使用默认模式，切换模式后，键值不对，程序会报错退出。

5、节点通讯图

终端输入，

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

如果一开始没有显示，选择【Nodes/Topics(all)】，然后点击左上角的刷新按钮。

6、代码解析

源码参考路径，

xxxxxxxxxx
/home/openeuler/slam_ws/src/yahboomcar_ctrl/yahboomcar_ctrl

yahboom_joy.py

​x
#创建订阅者订阅/joy话题数据
self.sub_Joy = self.create_subscription(Joy,'joy', self.buttonCallback,10)

#创建发布者发布速度、舵机、蜂鸣器话题数据以及功能玩法开关
self.pub_cmdVel = self.create_publisher(Twist,'cmd_vel',10)
self.pub_Buzzer = self.create_publisher(UInt16,"beep",  1)
self.pub_JoyState = self.create_publisher(Bool,"JoyState",  10)
self.pub_Servo1 = self.create_publisher(Int32,"servo_s1" , 10)
self.pub_Servo2 = self.create_publisher(Int32,"servo_s2" , 10)

#回调函数
def buttonCallback(self,joy_data):
    if not isinstance(joy_data, Joy): return
    self.user_jetson(joy_data)
    
#处理按键函数user_jetson
def user_jetson(self, joy_data):
    #cancel nav
    if joy_data.buttons[7] == 1: self.cancel_nav()
    #Buzzer 蜂鸣器控制
    if joy_data.buttons[11] == 1:
        b = UInt16()
        self.Buzzer_active = not self.Buzzer_active
        b.data = self.Buzzer_active 
        self.pub_Buzzer.publish(b)
   #档位调整，按下摇杆即可调节档位     
    xlinear_speed = self.filter_data(joy_data.axes[1]) * self.xspeed_limit * self.linear_Gear        
    angular_speed = self.filter_data(joy_data.axes[2]) * self.angular_speed_limit * self.angular_Gear
    if xlinear_speed > self.xspeed_limit: xlinear_speed = self.xspeed_limit
    elif xlinear_speed < -self.xspeed_limit: xlinear_speed = -self.xspeed_limit
    if angular_speed > self.angular_speed_limit: angular_speed = self.angular_speed_limit
    elif angular_speed < -self.angular_speed_limit: angular_speed = -self.angular_speed_limit
    twist = Twist()
    twist.linear.x = xlinear_speed
    twist.linear.y = 0.0
    twist.angular.z = angular_speed
    #判断是否能够控制速度，即R1键是否有按下
    if self.Joy_active == True:
        self.pub_cmdVel.publish(twist)
    #以下是处理舵机控制的数据，按下后角度会自加1/自减1，持续按下就持续的加减
    if not joy_data.buttons[1] == 0:
        print("Up")
        self.PWMServo_X += 1
        if self.PWMServo_X <= -90: self.PWMServo_X = -90
        elif self.PWMServo_X >= 90: self.PWMServo_X = 90
        print("self.PWMServo_X: ",self.PWMServo_X)
        print("self.PWMServo_Y: ",self.PWMServo_Y)
        servo1_angle = Int32()
        servo1_angle.data = self.PWMServo_X
        self.pub_Servo1.publish(servo1_angle)

    if not joy_data.buttons[3] == 0:
        print("Down")
        self.PWMServo_X -= 1
        if self.PWMServo_X <= -90: self.PWMServo_X = -90
        elif self.PWMServo_X >= 90: self.PWMServo_X = 90
        print("self.PWMServo_X: ",self.PWMServo_X)
        print("self.PWMServo_Y: ",self.PWMServo_Y)
        servo1_angle = Int32()
        servo1_angle.data = self.PWMServo_X
        self.pub_Servo1.publish(servo1_angle)

    if  not joy_data.buttons[0] == 0:
        print("Left")
        self.PWMServo_Y -= 1
        if self.PWMServo_Y <= -90: self.PWMServo_Y = -90
        elif self.PWMServo_Y >= 20: self.PWMServo_Y = 20
        servo2_angle = Int32()
        servo2_angle.data = self.PWMServo_Y
        self.pub_Servo2.publish(servo2_angle)
        print("self.PWMServo_X: ",self.PWMServo_X)
        print("self.PWMServo_Y: ",self.PWMServo_Y)

    if  not joy_data.buttons[4] == 0:
        print("Right")
        self.PWMServo_Y += 1
        if self.PWMServo_Y <= -90: self.PWMServo_Y = -90
        elif self.PWMServo_Y >= 20: self.PWMServo_Y = 20
        servo2_angle = Int32()
        servo2_angle.data = self.PWMServo_Y
        self.pub_Servo2.publish(servo2_angle)
        print("self.PWMServo_X: ",self.PWMServo_X)
        print("self.PWMServo_Y: ",self.PWMServo_Y)

7、遥控键值对应的变量

基于默认的模式【Controller】，遥控对应的键值如下，

结合上边的源码方便理解，当这些值改变的时候，说明遥控被按下，即可执行相对应的程序。查看其它的按键按下，可以订阅/joy话题，终端输入，

xxxxxxxxxx
ros2 topic echo /joy