jtson nano 手势控制ros小车

最新推荐文章于 2023-08-13 17:29:15 发布

chilian12321

最新推荐文章于 2023-08-13 17:29:15 发布

阅读量664

点赞数

文章标签： ubuntu linux Ros

本文链接：https://blog.csdn.net/chilian12321/article/details/126115427

版权

1.接线

5v---VCC

GND---GND

3---SDA

5---SCL

sudo apt-get update sudo apt-get install -y i2c-tools

检查安装情况，终端输入： apt-cache policy i2c-tools

输出如下即为安装成功

i2c-tools:

已安装： 4.0-2

候选： 4.0-2

版本列表：

*** 4.0-2 500 500 http://ports.ubuntu.com/ubuntu-ports bionic/universe arm64 Packages 1 00 /var/lib/dpkg/status

安装 smbus2，才能与python2兼容

Pypi上可以下载smbus2
smbus2`PyPi

点击下载
在这里插入图片描述

到下载的目录下解压
解压完：python setup.py install

gesture_control.launch文件

<launch>
    <!-- 开启机器人底层相关节点  -->
    <include file="$(find turn_on_wheeltec_robot)/launch/turn_on_wheeltec_robot.launch" />

    <node name="gesture_control" pkg="deskmedia" type="gesture_control.py" output="screen">
    <remap from="gesture_control/cmd_vel" to="/cmd_vel"/>
    </node>
</launch>

gesture_control.py文件

#!/usr/bin/python
# -*- coding:utf-8 -*-
import time
import smbus2
import sys
import rospy
import curses
from geometry_msgs.msg import Twist
#i2c地址
PAJ7620U2_I2C_ADDRESS   = 0x73
#寄存器Bank选择
PAJ_BANK_SELECT         = 0xEF          #Bank0== 0x00,Bank1== 0x01
#寄存器Bank 0
PAJ_SUSPEND             = 0x03      #I2C suspend命令(写= 0x01进入挂起状态)。I2C唤醒命令是奴隶ID唤醒。参考主题“I2C总线定时特性和协议”
PAJ_INT_FLAG1_MASK      = 0x41      #手势检测中断标志掩码
PAJ_INT_FLAG2_MASK      = 0x42      #手势/PS检测中断标志掩码
PAJ_INT_FLAG1           = 0x43      #手势检测中断标志
PAJ_INT_FLAG2           = 0x44      #手势/PS检测中断标志
PAJ_STATE               = 0x45      #手势检测状态指示灯(仅在手势检测模式下起作用)
PAJ_PS_HIGH_THRESHOLD   = 0x69      #PS迟滞高阈值(仅在接近检测模式下起作用)    
PAJ_PS_LOW_THRESHOLD    = 0x6A      #PS迟滞低阈值(仅在接近检测模式下起作用)
PAJ_PS_APPROACH_STATE   = 0x6B      #PS趋近状态，趋近= 1，(8位PS数据>= PS高门限)，不趋近= 0，(8位PS数据<= PS低门限)(仅在接近检测模式下有效)
PAJ_PS_DATA             = 0x6C      #PS 8位数据(仅在手势检测模式下有效)
PAJ_OBJ_BRIGHTNESS      = 0xB0      #物体亮度(最大255)
PAJ_OBJ_SIZE_L          = 0xB1      #对象大小(低8位)   
PAJ_OBJ_SIZE_H          = 0xB2      #对象大小(高8位)   
#寄存器Bank 1
PAJ_PS_GAIN             = 0x44      #PS增益设置(仅在接近检测模式下有效)
PAJ_IDLE_S1_STEP_L      = 0x67      #空闲S1步长，用于设置S1，响应系数(低8位)
PAJ_IDLE_S1_STEP_H      = 0x68      #空闲S1步长，用于设置S1，响应系数(高8位) 
PAJ_IDLE_S2_STEP_L      = 0x69      #空闲S2步长，用于设置S2，响应因子(低8位)
PAJ_IDLE_S2_STEP_H      = 0x6A      #空闲S2步长，用于设置S2，响应因子(高8位)
PAJ_OPTOS1_TIME_L       = 0x6B      #OPtoS1 Step，用于将操作状态的OPtoS1时间设置为待机1状态(低8位)
PAJ_OPTOS2_TIME_H       = 0x6C      #OPtoS1 Step，用于将OPtoS1运行状态时间设置为待机1 stateHigh 8位)  
PAJ_S1TOS2_TIME_L       = 0x6D      #S1toS2步，用于将待机1状态的S1toS2时间设置为待机2状态(低8位)  
PAJ_S1TOS2_TIME_H       = 0x6E      #S1toS2步，用于将待机1状态的S1toS2时间设置为待机2状态高8位) 
PAJ_EN                  = 0x72      #启用/禁用PAJ7620U2
#手势检测中断标志
PAJ_RIGHT               = 0x01 
PAJ_LEFT                = 0x02
PAJ_UP                  = 0x04 
PAJ_DOWN                = 0x08
PAJ_FORWARD             = 0x10 
PAJ_BACKWARD            = 0x20
PAJ_CLOCKWISE           = 0x40
PAJ_COUNT_CLOCKWISE     = 0x80
PAJ_WAVE                = 0x100

#启动初始化阵列
Init_Register_Array = (
    (0xEF,0x00),
    (0x37,0x07),
    (0x38,0x17),
    (0x39,0x06),
    (0x41,0x00),
    (0x42,0x00),
    (0x46,0x2D),
    (0x47,0x0F),
    (0x48,0x3C),
    (0x49,0x00),
    (0x4A,0x1E),
    (0x4C,0x20),
    (0x51,0x10),
    (0x5E,0x10),
    (0x60,0x27),
    (0x80,0x42),
    (0x81,0x44),
    (0x82,0x04),
    (0x8B,0x01),
    (0x90,0x06),
    (0x95,0x0A),
    (0x96,0x0C),
    (0x97,0x05),
    (0x9A,0x14),
    (0x9C,0x3F),
    (0xA5,0x19),
    (0xCC,0x19),
    (0xCD,0x0B),
    (0xCE,0x13),
    (0xCF,0x64),
    (0xD0,0x21),
    (0xEF,0x01),
    (0x02,0x0F),
    (0x03,0x10),
    (0x04,0x02),
    (0x25,0x01),
    (0x27,0x39),
    (0x28,0x7F),
    (0x29,0x08),
    (0x3E,0xFF),
    (0x5E,0x3D),
    (0x65,0x96),
    (0x67,0x97),
    (0x69,0xCD),
    (0x6A,0x01),
    (0x6D,0x2C),
    (0x6E,0x01),
    (0x72,0x01),
    (0x73,0x35),
    (0x74,0x00),
    (0x77,0x01),
)
#寄存器初始化数组
Init_PS_Array = (
    (0xEF,0x00),
    (0x41,0x00),
    (0x42,0x00),
    (0x48,0x3C),
    (0x49,0x00),
    (0x51,0x13),
    (0x83,0x20),
    (0x84,0x20),
    (0x85,0x00),
    (0x86,0x10),
    (0x87,0x00),
    (0x88,0x05),
    (0x89,0x18),
    (0x8A,0x10),
    (0x9f,0xf8),
    (0x69,0x96),
    (0x6A,0x02),
    (0xEF,0x01),
    (0x01,0x1E),
    (0x02,0x0F),
    (0x03,0x10),
    (0x04,0x02),
    (0x41,0x50),
    (0x43,0x34),
    (0x65,0xCE),
    (0x66,0x0B),
    (0x67,0xCE),
    (0x68,0x0B),
    (0x69,0xE9),
    (0x6A,0x05),
    (0x6B,0x50),
    (0x6C,0xC3),
    (0x6D,0x50),
    (0x6E,0xC3),
    (0x74,0x05),
)
#手势寄存器初始化数组
Init_Gesture_Array = (
    (0xEF,0x00),
    (0x41,0x00),
    (0x42,0x00),
    (0xEF,0x00),
    (0x48,0x3C),
    (0x49,0x00),
    (0x51,0x10),
    (0x83,0x20),
    (0x9F,0xF9),
    (0xEF,0x01),
    (0x01,0x1E),
    (0x02,0x0F),
    (0x03,0x10),
    (0x04,0x02),
    (0x41,0x40),
    (0x43,0x30),
    (0x65,0x96),
    (0x66,0x00),
    (0x67,0x97),
    (0x68,0x01),
    (0x69,0xCD),
    (0x6A,0x01),
    (0x6B,0xB0),
    (0x6C,0x04),
    (0x6D,0x2C),
    (0x6E,0x01),
    (0x74,0x00),
    (0xEF,0x00),
    (0x41,0xFF),
    (0x42,0x01),
)
class PAJ7620U2(object):
    def __init__(self,address=PAJ7620U2_I2C_ADDRESS):
        self._address = address
        self._bus = smbus2.SMBus(1)
        time.sleep(0.5)
        if self._read_byte(0x00) == 0x20:
            print("\nGesture Sensor OK\n")
            for num in range(len(Init_Register_Array)):
                self._write_byte(Init_Register_Array[num][0],Init_Register_Array[num][1])
        else:
            print("\nGesture Sensor Error\n")
        self._write_byte(PAJ_BANK_SELECT, 0)
        for num in range(len(Init_Gesture_Array)):
                self._write_byte(Init_Gesture_Array[num][0],Init_Gesture_Array[num][1])
    def _read_byte(self,cmd):
        return self._bus.read_byte_data(self._address,cmd)
    
    def _read_u16(self,cmd):
        LSB = self._bus.read_byte_data(self._address,cmd)
        MSB = self._bus.read_byte_data(self._address,cmd+1)
        return (MSB << 8) + LSB
    def _write_byte(self,cmd,val):
        self._bus.write_byte_data(self._address,cmd,val)
    def check_gesture(self):
        Gesture_Data=self._read_u16(PAJ_INT_FLAG1)
        if Gesture_Data == PAJ_UP:
            print("Up\r\n")
        elif Gesture_Data == PAJ_DOWN:
            print("Down\r\n")
        elif Gesture_Data == PAJ_LEFT:
            print("Left\r\n")   
        elif Gesture_Data == PAJ_RIGHT:
            print("Right\r\n")  
        elif Gesture_Data == PAJ_FORWARD:
            print("Forward\r\n")    
        elif Gesture_Data == PAJ_BACKWARD:
            print("Backward\r\n")
        elif Gesture_Data == PAJ_CLOCKWISE:
            print("Clockwise\r\n")  
        elif Gesture_Data == PAJ_COUNT_CLOCKWISE:
            print("AntiClockwise\r\n")  
        elif Gesture_Data == PAJ_WAVE:
            print("Wave\r\n")   
        return Gesture_Data
    def gesture_control(self,Gesture_Data,VV ,WW,times_loop):
        times_loop=50
        if Gesture_Data == PAJ_UP:
            VV=speed
            WW=0
        elif Gesture_Data == PAJ_DOWN:
            VV=-speed
            WW=0
        elif Gesture_Data == PAJ_LEFT:
            VV=0
            WW=turn
        elif Gesture_Data == PAJ_RIGHT:    
            VV=0
            WW=-turn  
        elif Gesture_Data == PAJ_FORWARD:
            VV=speed
            WW=0
        elif Gesture_Data == PAJ_BACKWARD:
            VV=-speed
            WW=0
        elif Gesture_Data == PAJ_CLOCKWISE:
            times_loop=100
            VV=0
            WW=-turn
        elif Gesture_Data == PAJ_COUNT_CLOCKWISE:
            times_loop=100
            VV=0
            WW=turn
        elif Gesture_Data == PAJ_WAVE:
            times_loop=0
            VV=0
            WW=0
        return     VV,WW,times_loop


# if __name__ == '__main__':
    
#     import time

#     print("\nGesture Sensor Test Program ...\n")

#     paj7620u2=PAJ7620U2()

#     while True:
#         time.sleep(0.05)
#         paj7620u2.check_gesture()



if __name__ == '__main__':
    
    import time

    print("\nGesture Sensor Test Program ...\n")

    paj7620u2=PAJ7620U2() #初始化IIC
    speed = 0.1 #默认移动速度 m/s 0.2
    turn  = 0.3   #默认转向速度 rad/s 1

    rospy.init_node('gesture_control')
    pub = rospy.Publisher('~cmd_vel', Twist, queue_size=5)#改成自己的topic
   
    r = rospy.Rate(10)   

    while True:
        r.sleep()
        Gesture_Data=paj7620u2.check_gesture()       
        VV=0
        WW=0
        times_loop=0
        VV ,WW,times_loop=paj7620u2.gesture_control(Gesture_Data,VV ,WW,times_loop)
        
        twist=Twist()
        twist.linear.x=VV
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = WW
        if(VV!=0 or WW!=0):
            print("VV=%f,WW=%f,times_loop=%d\r\n",VV,WW,times_loop) 
            for i in range(times_loop):
                pub.publish(twist) #ROS发布速度话题
                time.sleep(0.05)