精华内容
下载资源
问答
  • 最近工作上做了一个数据传输的小工具,使用UART传输,python语言写的,都知道python的跨平台特性,所以刚开始在自己PC上调试通过之后,想着万事大吉,把代码放到树莓派上面验证一下就可以了,谁知道事情并不是那么...

    最近工作上做了一个数据传输的小工具,使用UART传输,python语言写的,都知道python的跨平台特性,所以刚开始在自己PC上调试通过之后,想着万事大吉,把代码放到树莓派上面验证一下就可以了,谁知道事情并不是那么简单。

    4M的串口波特率,在PC上面的传输速度在180k/s,900k的文件大概就是5s左右传输完成,感觉这个速度还可以,到了树莓派上面,惊呆了,12k/s的速度,相差10倍还多。要知道产线上时间就是产能,产能就是金钱。所以花了很长的时间来找这个原因,刚开始怀疑树莓派内核版本太老,串口属性设置不对等等原因,到最后发现是因为CRC16算法问题。

    CRC算法,全称叫:循环冗余校验(Cyclic Redundancy Check, CRC)是一种根据网络数据包或计算机文件等数据产生简短固定位数校验码的一种信道编码技术,主要用来检测或校验数据传输或者保存后可能出现的错误。它是利用除法及余数的原理来作错误侦测的。

    因为传输数据需要确定数据在传输过程的准确性,所以需要用到校验算法来校验每包的数据。


    CRC16算法分为按位计算CRC和按字节计算CRC(查表法),刚开始我用的方法是按位计算的方法,代码如下:

    def CRC16(bytes):     
      wcrc = 0     
      byte_array = bytearray(bytes)
      for i in byte_array:
          c = i
          for _ in range(8):
              treat = c & 0x80
              c <<= 1
              bcrc = (wcrc >> 8) & 0x80
              wcrc <<= 1
              wcrc = wcrc & 0xffff           
              if (treat != bcrc):             
                  wcrc ^= 0x1021     
       return wcrc

    按位计算的优点在于不占空间,几乎没有内存的占用,但是缺点就是计算运算量比较大,花费时间较长


    通过逻辑分析仪抓取数据传输的波形发现,第一包数据发送结束,3ms会有一个ACK的回复,但是在ACK之后80ms,第二包数据才发出,所以这80ms的时间,大部分是用在了计算CRC上。

    因为我的PC是八核处理器,单核处理器在2.3G的主频,所以计算时间很快,这个问题几乎没有影响,但是树莓派是单核700M的主频,所以计算起来捉襟见肘。

    找到问题原因之后,开始解决问题,既然是运算能力不足,那就找一种运算量小的算法方式,那就是按字节计算(查表法),实现代码如下:

    const uint16_t crc_table[256] ={
    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 
    0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 
    0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 
    0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 
    0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 
    0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 
    0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 
    0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 
    0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, 
    0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 
    0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 
    0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 
    0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 
    0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 
    0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 
    0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 
    0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 
    0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 
    0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 
    0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 
    0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 
    0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 
    0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 
    0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 
    0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 
    0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 
    0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 
    0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 
    0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 
    0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 
    0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 
    0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
    
    uint16_t crc_16(uint8_t *data, uint16_t len)
    {
        uint16_t crc16 = 0x0000;
        uint16_t crc_h8, crc_l8;
        
        while( len-- ) {
            crc_h8 = (crc16 >> 8);
            crc_l8 = (crc16 << 8);
            crc16 = crc_l8 ^ crc_table[crc_h8 ^ *data];
            data++;
        }
        return crc16;
    }

    按字节计算的方式优点是运算量小,缺点是占用内存空间大,但是树莓盘1G的内存,而我只关心计算速度,内存消耗不是关注点,所以按字节计算满足我的需求。


    以上只是C代码,但是我的工具为了跨平台使用的python,所以需要在python下引用C代码具体方法是:

    创建CRC.c文件,实现CRC算法
    使用GCC编译器编译特定平台的库文件
      e.g:arm-linux-gnueabihf-gcc -shared -Wl,-soname,adder -o crc.so -fPIC CRC.c
      生成crc.so文件
    python代码中import ctype 模块
    引用C代码实现的函数
    完成

    最终的实现代码如下:

    import ctype
    adder = ctypes.cdll.LoadLibrary('./crc.so')
    
    def CRC16(bytes):
         wcrc = 0
         byte_array = bytearray(bytes)
         wcrc = adder.CRC_16(bytes,len(byte_array))
         wcrc = wcrc & 0xFFFF
         return wcrc

    这个问题困扰了我很久,因为刚开始怀疑内核版本太老,因为我的虚拟机的内核版本是5.4,但是树莓派是4.14版本,然后从树莓派官网下载工具和源码,编译的最新的5.4内核装机之后还是不行,编译内核和修改内核配置的历程真是艰难,尤其是编译完成,内核更新之后速度依旧很慢。

     还好最后定位到真正原因,找到了解决问题的办法,才有了这篇文章,有时间会梳理一下树莓派裁剪编译内核的步骤写成文章分享出来。

    展开全文
  • 树莓派:MPU6050 欧拉角算法程序

    千次阅读 2019-07-05 19:47:17
    #IMU算法更新 Kp = 100 #比例增益控制加速度计/磁强计的收敛速度 Ki = 0.002 #积分增益控制陀螺偏差的收敛速度 halfT = 0.001 #采样周期的一半 #传感器框架相对于辅助框架的四元数(初始化四元数的值) q0 = 1 q1 =...

    通过x、y、z加速度和陀螺仪计算姿态角(欧拉角)

    #coding:utf-8
    
    import math
    
    #IMU算法更新
    
    
    Kp = 100 #比例增益控制加速度计/磁强计的收敛速度
    Ki = 0.002 #积分增益控制陀螺偏差的收敛速度
    halfT = 0.001 #采样周期的一半
    
    #传感器框架相对于辅助框架的四元数(初始化四元数的值)
    q0 = 1
    q1 = 0
    q2 = 0
    q3 = 0
    
    #由Ki缩放的积分误差项(初始化)
    exInt = 0
    eyInt = 0
    ezInt = 0
    
    def IMUupdate(ax,ay,az,gx,gy,gz):
        global q0
        global q1
        global q2
        global q3
        global exInt
        global eyInt
        global ezInt
        # print(q0)
        
        #测量正常化
        norm = math.sqrt(ax*ax+ay*ay+az*az)
        #单元化
        ax = ax/norm
        ay = ay/norm
        az = az/norm
        
        #估计方向的重力
        vx = 2*(q1*q3 - q0*q2)
        vy = 2*(q0*q1 + q2*q3)
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3
        
        #错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
        ex = (ay*vz - az*vy)
        ey = (az*vx - ax*vz)
        ez = (ax*vy - ay*vx)
        
        #积分误差比例积分增益
        exInt += ex*Ki
        eyInt += ey*Ki
        ezInt += ez*Ki
        
        #调整后的陀螺仪测量
        gx += Kp*ex + exInt
        gy += Kp*ey + eyInt
        gz += Kp*ez + ezInt
        
        #整合四元数
        q0 += (-q1*gx - q2*gy - q3*gz)*halfT
        q1 += (q0*gx + q2*gz - q3*gy)*halfT
        q2 += (q0*gy - q1*gz + q3*gx)*halfT
        q3 += (q0*gz + q1*gy - q2*gx)*halfT
        
        #正常化四元数
        norm = math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
        q0 /= norm
        q1 /= norm
        q2 /= norm
        q3 /= norm
        
        #获取欧拉角 pitch、roll、yaw
        pitch = math.asin(-2*q1*q3+2*q0*q2)*57.3
        roll = math.atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3
        yaw = math.atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3
        return pitch,roll,yaw
    

    --------------------- 
    作者:溺水咸鱼 
    来源:CSDN 
    原文:https://blog.csdn.net/Forter_J/article/details/85278335 
    版权声明:本文为博主原创文章,转载请附上博文链接!

    展开全文
  • # 第一行引用后,可以设置中文注释 树莓派上才有的 import time import sys import random gpio . setwarnings ( False ) # 去掉一些不必要的警告 def init ( ) : gpio . setmode ( gpio . BOARD ...

    请大家先看看我的法拉利~
    在这里插入图片描述
    在这里插入图片描述
    它身上有前中后三个超声波传感器,轻松调用他们就能使小车避障轻松走出迷宫啦。预期小车走的路线是这样子的:
    在这里插入图片描述
    我的核心思路:
    初始小车为前进状态,此时使用前、左、右三个传感器检测小车三边与障碍物的距离,当前方距离小于一个界限值时,判断左右传感器与障碍物的距离,如果左大于右,则向左转,否则,向右。当转向完成后小车的转向角度可能会发生偏差,判断左右传感器与障碍物的距离,设置一个大致的调整时间,使小车停止,然后进行微调整,再返回循环体继续前进。

    使小车自己进行微微调整的思路:
    转向完成后,判断左右传感器与障碍物的距离,如果小于一个界限值,则令小车进行微调。使小车有一个较好的角度前进,将碰撞的可能性降到最低。

    程序流程图:
    在这里插入图片描述
    代码如下:

    # -*- coding: utf-8 -*-
    
    import RPi.GPIO as gpio  # 第一行引用后,可以设置中文注释  树莓派上才有的
    import time
    import sys
    import random
    
    gpio.setwarnings(False)  # 去掉一些不必要的警告
    
    
    def init():
        gpio.setmode(gpio.BOARD)  # GPIO调用BOARD编号方式
        gpio.setup(38, gpio.OUT)
        gpio.setup(40, gpio.IN)
        gpio.setup(35, gpio.OUT)
        gpio.setup(37, gpio.IN)
        gpio.setup(29, gpio.OUT)
        gpio.setup(31, gpio.IN)
    
    
    def gp():
        gpio.setmode(gpio.BOARD)
        gpio.setup(12, gpio.OUT)  # 引脚设置为输出
        gpio.setup(16, gpio.OUT)
        gpio.setup(18, gpio.OUT)
        gpio.setup(22, gpio.OUT)
    
    
    # 前传感器
    def forward_distance():
        init()
        gpio.output(38, True)  # 发出触发信号保持10us以上(15us)
        time.sleep(0.000015)
        gpio.output(38, False)
        while not gpio.input(40):
            pass
        t1 = time.time()  # 发现高电平时开时计时
        while gpio.input(40):
            pass
        t2 = time.time()  # 高电平结束停止计时
        return (t2 - t1) * 34000 / 2  # 返回距离,单位为厘米
        gpio.cleanup()
        return forward_distance
    
    
    # 左传感器
    def left_distance():
        init()
        gpio.setup(35, gpio.OUT)
        gpio.output(35, True)  # 发出触发信号保持10us以上(15us)
        time.sleep(0.000015)
        gpio.output(35, False)
        while not gpio.input(37):
            pass
        t1 = time.time()  # 发现高电平时开时计时
        while gpio.input(37):
            pass
        t2 = time.time()  # 高电平结束停止计时
        return (t2 - t1) * 34000 / 2  # 返回距离,单位为厘米
        gpio.cleanup()
        return left_distance
    
    
    # 右传感器
    def right_distance():
        init()
        gpio.output(29, True)  # 发出触发信号保持10us以上(15us)
        time.sleep(0.000015)
        gpio.output(29, False)
        while not gpio.input(31):
            pass
        t1 = time.time()  # 发现高电平时开时计时
        while gpio.input(31):
            pass
        t2 = time.time()  # 高电平结束停止计时
        return (t2 - t1) * 34000 / 2  # 返回距离,单位为厘米
        gpio.cleanup()
        return right_distance
    
    
    def forward():
        init()
        gp()
        # 全速前进状态
        gpio.output(12, True)  # 左前进
        gpio.output(16, False)  # 左后退
        gpio.output(22, False)  # 右后退
        gpio.output(18, True)  # 右前进
        # q = gpio.PWM(12, 50)  # !error 两个PWM信号程序会出错
        # p = gpio.PWM(18, 50)  # 信道,频率
        # q.start(50)
        # p.start(50)  # 占空比
        time.sleep(0)
    
    
    # 左转
    def left(runtime):
        gp()
        gpio.output(12, False)
        gpio.output(16, False)
        gpio.output(22, False)
        gpio.output(18, True)
        p = gpio.PWM(18, 80)  # 信道,频率
        p.start(50)  # 占空比
        time.sleep(runtime)
        gpio.cleanup()
    
    
    # 右转
    def right(runtime):
        gp()
        gpio.output(12, True)
        gpio.output(16, False)
        gpio.output(22, False)
        gpio.output(18, False)
        p = gpio.PWM(12, 80)  # 信道,频率
        p.start(50)  # 占空比
        time.sleep(runtime)
        gpio.cleanup()
    
    
    # 左边微调
    def adjust_left(runtime):
        init()
        gp()
        gpio.output(12, False)  # 左前进
        gpio.output(16, True)  # 左后退
        gpio.output(22, False)  # 右后退
        gpio.output(18, True)  # 右前进
        # q = gpio.PWM(16,50)
        # p = gpio.PWM(18,50)
        # q.start(50)
        # p.start(50)
        time.sleep(runtime)
    
    
    # 右边微调
    def adjust_right(runtime):
        init()
        gp()
        gpio.output(12, True)  # 左前进
        gpio.output(16, False)  # 左后退
        gpio.output(22, True)  # 右后退
        gpio.output(18, False)  # 右前进
        # q = gpio.PWM(12, 50)
        # p = gpio.PWM(22, 50)
        # q.start(50)
        # p.start(50)
        time.sleep(runtime)
    
    
    def stop(runtime):
        init()
        gp()
        gpio.output(12, False)
        gpio.output(16, False)
        gpio.output(22, False)
        gpio.output(18, False)
        gpio.cleanup()
        time.sleep(runtime)
    
    
    def run():
        while True:
            init()
            gp()
            forward()
            fd = forward_distance()
            ld = left_distance()
            rd = right_distance()
            print('前距离是:%0.2f cm' % fd)
            print("----------------------")
            if ld < 7:
                stop(0.3)
                adjust_left(0.5)
                print("向左微转")
                continue
            if rd < 7:
                stop(0.3)
                adjust_right(0.5)
                print("向右微转")
                continue
            if fd > 200:
                stop(0.3)
                adjust_left(0.5)
                continue
            if fd < 30 and ld > rd:
                print("左转")
                print("----------------------")
                print('左距离是:%0.2f cm' % ld)
                left(2)
                continue
            elif fd < 30 and rd > ld:
                print("右转")
                print("----------------------")
                print('右距离是:%0.2f cm' % rd)
                right(1.25)
                continue
            else:
                continue
    
    
    run()
    

    总体是非常简单的,代码也有注释,希望对你们有帮助!

    展开全文
  • 树莓派控制无人机实现定点降落:地标识别算法以及控制算法

    树莓派控制无人机实现定点降落(六)——地标识别及控制算法的实现

    地标识别方法同样参照了我上篇博文里提到的另一个人的博文的方法,但稍有改动,在这给出参考博文的链接
    链接在此
    本篇文章最后的运行效果我发布到了b站,大家可以去看看[传送门]

    通过我上篇博文的介绍,地标识别只需要识别嵌套最多的矩形即可,而树莓派安装opencv比较麻烦,所以地标识别部分我使用cv2写,并将识别好的矩形中心点发布到ros节点上用于控制。
    注:树莓派快速安装cv2:

    sudo apt-get install python-opencv
    

    使用pip安装将很慢,这种安装方法虽然版本久了点,但还是可以用的。
    接下来实现地标识别

    1、地标识别

    使用轮廓提取,多边形抽象,过滤面积较小的图形,然后过滤出四边形,再过滤掉非凸形。得到的四边形里通过简单的聚类方法寻找中心距离最近的一类,其中心的平均值即为地标中心。
    (1)首先,建立工作空间

    mkdir -p ~/catkin_ws/src
    cd ~/catkin_ws/src
    catkin_create_pkg landing roscpp rospy std_msgs geometry_msgs mavros
    cd ~/catkin_ws/src/landing
    mkdir msg #存放自定义消息类型
    mkdir scripts #存放python脚本
    

    (2)自定义消息类型
    由cv2识别出的矩形中心点将依照某种自定义格式发布到topic上,自定义center消息类型如下:
    在msg文件夹下创建center.msg文件,消息格式我定义为图片的宽和高,矩形中心的位置,以及是否检测到矩形,内容如下:

    uint32 width
    uint32 height
    float64 x
    float64 y
    bool iffind
    

    (3)创建识别脚本
    在scripts下创建track.py。
    原理上面讲了,直接上代码:

    import rospy
    from std_msgs.msg import Header
    from sensor_msgs.msg import Image
    from landing.msg import center # 自定义消息类型
    import os
    import cv2
    import numpy as np
    import time
    
    center_publish=rospy.Publisher('/center', center, queue_size=1) # 发布矩形中心
    # 获得摄像头图像的回调函数
    def callback(Image):
        img = np.fromstring(Image.data, np.uint8)
        img = img.reshape(240,320,3)
        track(img, Image.width, Image.height) # 寻找矩形
    
    # 订阅获得摄像头图像
    def listener():
        rospy.init_node('track')
        rospy.Subscriber('/iris/usb_cam/image_raw', Image, callback)
        rospy.spin()
    
    # 寻找矩形中心
    def track(frame, width, height):
        img = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
        _, img = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
        contours = cv2.findContours(img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) # 轮廓提取
        rects = [] # 存放四边形
        centers = [] # 存放中心点
        for contour in contours[1]:
            if cv2.contourArea(contour) < 100: # 过滤掉矩形面积小的
                continue
            epsilon = 0.02 * cv2.arcLength(contour, True)
            approx = cv2.approxPolyDP(contour, epsilon, True)
            if approx.shape[0] == 4 and cv2.isContourConvex(approx): # 过滤掉非四边形与非凸形
                rects.append(approx)
                centers.append((approx[0]+approx[1]+approx[2]+approx[3]).squeeze()/4)
    
    	# 以下部分为聚类算法
        center_iter = list(range(len(centers)))
        result = []
        threshold = 20
        while len(center_iter) is not 0:
            j = 1
            resultnow = []
            while j < len(center_iter):
                if np.sum((centers[center_iter[0]] - centers[center_iter[j]])**2) < threshold:
                    resultnow.append(center_iter[j])
                    center_iter.pop(j)
                    j = j-1
                j = j+1
            resultnow.append(center_iter[0])
            center_iter.pop(0)
            if len(result) < len(resultnow):
                result = resultnow
        rects = np.array(rects)[result]
        # 如果嵌套的矩形数量大于2才算提取成功
        if len(result) > 2:
    		centers = np.sum(np.array(centers)[result], axis=0).astype(np.double) / len(result)
    		publish(centers, width, height) # 发布消息
        else:
        	center_temp = center()
            center_temp.iffind = False
    		center_publish.publish(center_temp)
        
        # 下面注释掉的部分将画出提取出的轮廓
        #cv2.polylines(frame, rects, True, (0,0,255), 2)
        #cv2.imshow('w',frame)
        #cv2.waitKey(1)
    
    # 发布中心点消息
    def publish(centers, width, height):
        center_temp = center()
        center_temp.width = width
        center_temp.height = height
        center_temp.x = centers[1]
        center_temp.y = centers[0]
        center_temp.iffind = True
        center_publish.publish(center_temp)
    
    if __name__ == '__main__':
        listener()
    

    下面是识别效果:
    在这里插入图片描述

    2、控制算法

    通过矩形中心点距离图片中心的距离控制,使用pid算法:

    #include <ros/ros.h>
    #include <geometry_msgs/PoseStamped.h>
    #include <geometry_msgs/TwistStamped.h>
    #include <mavros_msgs/CommandBool.h>
    #include <mavros_msgs/SetMode.h>
    #include <mavros_msgs/State.h>
    #include <landing/center.h> // 导入自定义消息类型
    
    mavros_msgs::State current_state;
    void state_cb(const mavros_msgs::State::ConstPtr& msg){
        current_state = *msg;
    }
    
    geometry_msgs::PoseStamped local_position;
    void position_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
        local_position = *msg;
    }
    
    landing::center landmark;
    void center_cb(const landing::center::ConstPtr& msg){
        landmark = *msg;
    }
    
    int main(int argc, char **argv)
    {
        ros::init(argc, argv, "landing_node");
        ros::NodeHandle nh;
     	
        ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
                ("mavros/state", 10, state_cb);
        ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
                ("mavros/local_position/pose", 10, position_cb);
        ros::Subscriber center_sub = nh.subscribe<landing::center>
                ("center", 10, center_cb);
        ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
                ("mavros/setpoint_position/local", 10);
        ros::Publisher local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>
                ("mavros/setpoint_velocity/cmd_vel", 10);
        ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
                ("mavros/cmd/arming");
        ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
                ("mavros/set_mode");
     
        ros::Rate rate(20.0);
     
        while(ros::ok() && current_state.connected){
            ros::spinOnce();
            rate.sleep();
        }
     
        geometry_msgs::PoseStamped pose;//姿态控制
        pose.pose.position.x = 0;
        pose.pose.position.y = 0;
        pose.pose.position.z = 2;
        
        geometry_msgs::TwistStamped vel;//速度控制
     
        for(int i = 100; ros::ok() && i > 0; --i){
            local_pos_pub.publish(pose);
            ros::spinOnce();
            rate.sleep();
        }
     
        mavros_msgs::SetMode offb_set_mode;
        offb_set_mode.request.custom_mode = "OFFBOARD";
     
        mavros_msgs::CommandBool arm_cmd;
        arm_cmd.request.value = true;
     
        ros::Time last_request = ros::Time::now();
     	
     	//起飞
        while(ros::ok())
        {
            if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
                {
                    ROS_INFO("Offboard enabled");
                }
               	last_request = ros::Time::now();
           	}
            else if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
            else if(ros::Time::now() - last_request > ros::Duration(5.0))
            	break;
     
            local_pos_pub.publish(pose);
     
            ros::spinOnce();
            rate.sleep();
        }
        
        //逛一圈
    	last_request = ros::Time::now();
    	while(ros::ok())
    	{
    		if(ros::Time::now() - last_request > ros::Duration(5.0))
    			break;
    	    vel.twist.linear.x = 1;
    		vel.twist.linear.y = 0;
    		vel.twist.linear.z = 0;
    		local_vel_pub.publish(vel);
    		ros::spinOnce();
        	rate.sleep();
    	}
    	last_request = ros::Time::now();
    	while(ros::ok())
    	{
    		if(ros::Time::now() - last_request > ros::Duration(5.0))
    			break;
    	    vel.twist.linear.x = 0;
    		vel.twist.linear.y = 1;
    		vel.twist.linear.z = 0;
    		local_vel_pub.publish(vel);
    		ros::spinOnce();
        	rate.sleep();
    	}
    	last_request = ros::Time::now();
    	while(ros::ok())
    	{
    		if(ros::Time::now() - last_request > ros::Duration(5.0))
    			break;
    	    vel.twist.linear.x = -1;
    		vel.twist.linear.y = 0;
    		vel.twist.linear.z = 0;
    		local_vel_pub.publish(vel);
    		ros::spinOnce();
        	rate.sleep();
    	}
    	last_request = ros::Time::now();
    	while(ros::ok())
    	{
    		if(ros::Time::now() - last_request > ros::Duration(5.0))
    			break;
    	    vel.twist.linear.x = 0;
    		vel.twist.linear.y = -1;
    		vel.twist.linear.z = 0;
    		local_vel_pub.publish(vel);
    		ros::spinOnce();
        	rate.sleep();
    	}
    	
    	//控制降落部分
    	while(ros::ok())
    	{
    		//高度低于0.3时转为降落模式
    		if(local_position.pose.position.z < 0.3)
    			break;
    		//如果找到地标,控制方向
    		if(landmark.iffind)
    		{
    			//计算两方向err
    			double err_x = landmark.height/2.0 - landmark.x;
    			double err_y = landmark.width/2.0 - landmark.y;
    			ROS_INFO_STREAM("state="<<err_x<<" "<<err_y);
    			//速度控制
    			vel.twist.linear.x = err_x/400;
    			vel.twist.linear.y = err_y/400;
    			//如果位置很正开始降落
    			if(err_x < 10 && err_y < 10)
    				vel.twist.linear.z = -0.2;
    			else
    				vel.twist.linear.z = 0;
    			local_vel_pub.publish(vel);
    			ros::spinOnce();
    			rate.sleep();
        	}
        	//如果找不到矩形地标,回到2m高度
        	else
        	{
        		pose.pose.position.x = local_position.pose.position.x;
    			pose.pose.position.y = local_position.pose.position.y;
    			pose.pose.position.z = 2;
    			local_pos_pub.publish(pose);
    			ros::spinOnce();
    			rate.sleep();
        	}
    	}
        
        offb_set_mode.request.custom_mode = "AUTO.LAND";
        if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
        {
            ROS_INFO("Offboard enabled");
            last_request = ros::Time::now();
        }
     
        return 0;
    }
    

    这个代码的问题在于找不到地标的情况不够完整,后续我会改进。

    3、剩余工作

    CmakeLists.txt
    需要注意添加的我已经用注释标出

    cmake_minimum_required(VERSION 2.8.3)
    project(landing)
    find_package(catkin REQUIRED COMPONENTS
      mavros
      message_generation
      roscpp
      rospy
      std_msgs
    )
    add_message_files(FILES center.msg) # 这句话要注意添加
    generate_messages(DEPENDENCIES std_msgs) # 这句话要注意添加
    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES landing
    #  CATKIN_DEPENDS roscpp rospy std_msgs
    #  DEPENDS system_lib
    )
    include_directories(
    # include
      ${catkin_INCLUDE_DIRS}
    )
    add_executable(landing_node src/control.cpp) # 这句话要注意添加
    # 这句话要注意添加
    target_link_libraries(landing_node 
    ${catkin_LIBRARIES}
    )
    
    

    package.xml
    需要注意的地方我已经用注释标出

    <?xml version="1.0"?>
    <package format="2">
      <name>landing</name>
      <version>0.0.0</version>
      <description>The landing package</description>
      <maintainer email="japdzbq@126.com">cyrilsterling</maintainer>
      <license>TODO</license>
        <buildtool_depend>catkin</buildtool_depend>
      <build_depend>roscpp</build_depend>
      <build_depend>rospy</build_depend>
      <build_depend>std_msgs</build_depend>
      <build_depend>geometry_msgs</build_depend>
      <build_depend>mavros</build_depend>
      <!-- 这句话要注意添加 -->
      <build_depend>message_generation</build_depend>
      <build_export_depend>roscpp</build_export_depend>
      <build_export_depend>rospy</build_export_depend>
      <build_export_depend>std_msgs</build_export_depend>
      <build_export_depend>geometry_msgs</build_export_depend>
      <build_export_depend>mavros</build_export_depend>
      <exec_depend>roscpp</exec_depend>
      <exec_depend>rospy</exec_depend>
      <exec_depend>std_msgs</exec_depend>
      <exec_depend>geometry_msgs</exec_depend>
      <exec_depend>mavros</exec_depend>
      <!-- 这句话要注意添加 -->
      <exec_depend>message_generation</exec_depend>
      <export>
      </export>
    </package>
    

    之后返回catkin_ws编译运行就好,记得source,运行顺序如下:

    roslaunch px4 mavros_posix_sitl.launch
    python src/landing/scripts/track.py
    rosrun landing landing_node
    

    也可以自己写一个launch文件一次性运行
    仿真到此结束,下一篇将是实物尝试与运用在实物上时算法的变化
    希望疫情早点结束鸭!

    展开全文
  • 本文经AI新媒体量子位(公众号 ID: QbitAI)授权转载,转载请联系出处 本文约1300字,建议阅读5分钟本文介绍了使用树莓派复古相机开源自己编写算法的故事。 手机拍照不够爽,带个...
  • 使用树莓派实现的口罩检测

    万次阅读 多人点赞 2020-05-17 11:48:53
    使用的口罩检测 项目是AIZOO团队实现的 使用的是目标检测常用的SSD算法。 (由于疫情在家 连不上实验室的服务器我无法训练) 项目GitHub链接 AIZOO团队给出的 代码在Windows上很容易以跑通。配置的环境相对比较容易...
  • 萧箫 发自 凹非寺 量子位 报道 | 公众号 QbitAI手机拍照不够爽,带个单反又太重?试试做个树莓派复古相机,还能自己编写处理算法的那种——成本不到700元。没错,颜值很高,拍出来的照...
  • 手机拍照不够爽,带个单反又太重?试试做个树莓派复古相机,还能自己编写处理算法的那种——成本不到700元。没错,颜值很高,拍出来的照片也能打:你也可以快速上手做一个。如何制作一个树莓派复古相...
  • 树莓派ROS机器人

    千次阅读 2020-11-04 17:52:00
    ROS机器人总结笔记连接树莓派修改IP地址开启底盘节点校准IMU调用键盘:角速度校正鼠标构建地图保存地图自动导航与自动避障使用Hector算法构建地图(会有重影,不适合障碍物多的地方)>动态调节PID参数摄像头寻线 与...
  • 树莓派OpenCV镜像

    热门讨论 2020-11-04 23:17:34
    OpenCV是一个基于BSD许可发行的跨...它轻量级而且高效,同时提供了Python、Ruby、MATLAB等语言的接口,实现了图像处理和计算机视觉方面的很多通用算法。 为了方便在树莓派上使用,我制作了一个OpenCV-4.5.0直刷镜像。
  • 选择会话--属性-SSH-安全性--加密--编辑,勾选aes256-ctr,aes128-ctr,aes2192-ctr其中之一即可
  • 本代码是支持树莓派的opencv显示,在树莓派操作系统上已经运行成功,属于基础入门,想要确定怎么使用opencv,调用的是边缘算法,从而使官方摄像头和opencv连接起来,欢迎向我咨询
  • PaddlePaddle学习之使用PaddleDetection在树莓派4B进行模型部署(三)----- 树莓派4B...本文将使用ssd_mobilenet_v1_voc算法,以一个例子为说明如何利用paddleDetection完成一个项目----从准备数据集到完成树莓派部...
  • 基于树莓派3B+的实时多线程人脸haar检测算法。 使用opencv+python编写 操作系统:ubuntu mate 18.04
  • 树莓派IO口驱动

    2021-03-22 18:59:34
    (3)虚拟地址:逻辑(基于算法的地址(软件层面的地址,假))地址称为逻辑地址 MMU的作用:将物理地址转化为虚拟地址 2.寄存器 翻阅【树莓派博通BCM2835芯片手册】第6章(89页): The GPIO has 41 registers. All...
  • 树莓派3B--搭建dlib

    千次阅读 2018-01-04 09:21:57
    1、树莓派3b; 2、ubuntu mate系统 linux内核是4.4.38;   功能需求: 1、调用dlib实现人脸识别。     可行性研究: 1、dlib安装的库有多大? 分析:windows下dlib19.7版本,debug版本65M、release版本...
  • 树莓派4b+A111毫米波雷达 呼吸算法demo 树莓派4b+A111毫米波雷达 button press demo 树莓派4b+A111毫米波雷达 obstacles section demo 树莓派4b+A111毫米波雷达 呼吸算法demo 树莓派4b+A
  • 树莓派安装numpy,pandas等库

    千次阅读 2020-08-26 21:17:38
    这两天在学习树莓派系统的使用,以便于用python写个跌倒检测识别算法。数据采集用树莓派外接MPU6050陀螺仪模块,用无线模块或者蓝牙模块(树莓派好像是自带的)传输到PC,在PC上做数据处理以及模型训练。因此,即使不...

空空如也

空空如也

1 2 3 4 5 ... 14
收藏数 263
精华内容 105
关键字:

树莓派算法