【鲁班猫创意大赛】有烟无害环境卫士（鲁班猫1s做ros主控）

卿九金

大佬们好，分享一下我用鲁班猫做ROS主控，stm32f407做底层驱动做的一个ros小车。

目的是识别烟雾并净化：净化是用的负离子发生器，外加扇叶将其扩散出去。同时也具有环境气体浓度（质量）检测的功能。

这是我去年12月底开始做的，入坑鲁班猫算是比较早了。在读大三学生。正奥里给考研中。。。
最底下还塞了一块vet6和一块esp32.
项目在电子发烧友的链接：https://bbs.elecfans.com/jishu_2344500_1_1.html

项目视频链接：https://www.bilibili.com/video/BV1hh4y1n7Fz/?vd_source=4fa660ff7e4423139e6ebdbd4dece6c7

加了个散热风扇

以下是功能层的32部分，鲁班猫是用于ros建图和导航。

先介绍功能部分即功能层的stm32与串口屏、esp32通信部分。功能层的主要目的是获取传感器数据和通过继电器控制小车前端的负离子发生器和两个加快负离子扩散的风扇。

注：这里的stm32相当于一个中转，用的是rt—thread实时操作系统，版本是4.0.2（写的比较早，当时rtt有bug。）

因为用的是rtt，移植性很高，故只写了应用层的main.c函数。如下：气体传感器如下（所用的是串口协议）

开启三个串口：一个用于读取传感器，一个用于接收和发送指令给串口屏，一个用于给esp32传输数据，通过esp32将数据发送到巴法云平台，做接入小程序中转。篇幅有限，代码放在了网盘上。链接：https://pan.baidu.com/s/1ltgypPMq9heezk412r4IKw?pwd=jhzs提取码：jhzs
整体流程图如下：

1. #include <rtthread.h>
   
2. /*串口1用来调试*/
   
3. #define DBG_TAG "main"
   
4. #define DBG_LVL DBG_LOG
   
5. #include <rtdbg.h>
   
6. #include <string.h>
   
7. #include <serial.h>  //此处有坑，要改头文件路径为rt-thread/components/drivers/include/drivers
   
8. #include <stdio.h>
   
9. #include "stdlib.h"
   
10. 
    
11. #define key1_open  rt_pin_write(51,PIN_LOW );//d3
    
12. #define key1_close  rt_pin_write(51,PIN_HIGH );
    
13. 
    
14. #define key2_open  rt_pin_write(52,PIN_LOW );//d4
    
15. #define key2_close  rt_pin_write(52,PIN_HIGH );
    
16. 
    
17. #define key3_open  rt_pin_write(53,PIN_LOW );//d5
    
18. #define key3_close  rt_pin_write(53,PIN_HIGH );
    
19. 
    
20. rt_thread_t  majoy_th;
    
21. 
    
22. /*micropython esp32与rtt串口DMA传输数据时有坑，
    
23. * 需在drv_usart.c找到HAL_UART_RxCpltCallback和HAL_UART_RxHalfCpltCallback将dma_isr(&uart->serial)注释掉，
    
24. * 能降低数据错误率*/
    
25. 
    
26. /*串口2的变量 115200*/
    
27. struct serial_configure  uar2_configs = RT_SERIAL_CONFIG_DEFAULT;
    
28. rt_sem_t sem2;
    
29. rt_device_t uar2_dev;
    
30. rt_thread_t uar_2_th;
    
31. rt_thread_t uar_2_deal;
    
32. char buffer[256] = {0};
    
33. rt_size_t rxlen2 = 0;
    
34. 
    
35. /*串口3的变量 9600*/
    
36. struct serial_configure  uar3_configs = MY_SERIAL_CONFIG_DEFAULT;
    
37. rt_sem_t sem3;
    
38. rt_device_t uar3_dev;
    
39. rt_thread_t uar_3_th;
    
40. uint8_t buffer3[17] = {0};
    
41. rt_size_t rxlen3 = 0;
    
42. 
    
43. /*串口4的变量 115200*/
    
44. struct serial_configure  uar4_configs = RT_SERIAL_CONFIG_DEFAULT;
    
45. rt_sem_t sem4;
    
46. rt_device_t uar4_dev;
    
47. rt_thread_t uar_4_th;
    
48. rt_uint8_t buffer4[256] = {0xff};
    
49. rt_size_t rxlen4 = 0;
    
50. //char deal;
    
51. 
    
52. rt_uint8_t deal ;
    
53. char wheater[8];
    
54. char humidity[4];
    
55. char temperature[4];
    
56. char wind_speed[6];
    
57. char shi[3];
    
58. char miao[3];
    
59. char fen[3];
    
60. 
    
61. char wheater_deal[21="main2.g3.txt="";
    
62. char humidity_deal[17="main2.g1.txt="";
    
63. 
    
64. char shi_deal[15] = "main.z1.val=";
    
65. char miao_deal[15] = "main.z0.val=";
    
66. char end[1=""";
    
67. char xf_end[3];
    
68. 
    
69. void uar2_thread_entry(void *parameter)//串口2DMA线程入口
    
70. {
    
71.     rt_size_t len = 0;
    
72. 
    
73.     char temperature_deal[17="main2.g0.txt="";
    
74.     char fen_deal[16] = "main.z2.val=";
    
75.     while(1)
    
76.     {
    
77. 
    
78.         rt_sem_take(sem2, RT_WAITING_FOREVER);
    
79.         char wind_speed_deal[19="main2.g2.txt="";
    
80. 
    
81.         len = rt_device_read(uar2_dev, 0, buffer, rxlen2);
    
82.         buffer[len] ='\0';
    
83. 
    
84.           rt_kprintf(buffer);
    
85. 
    
86. ....(部分代码)
    

复制代码

接下来是鲁班猫1s做ros主控的部分。1、移植轮趣大佬的ros源码：根据我现有的硬件：思岚a1雷达、一个usb rgb摄像头选择合适的功能包，然后开始移植。中途会出现很多错误。例如缺少部分功能包，sudo apt install ros-noetic-(包名)【我的ros版本是noetic】。2、移植完毕后发现大佬们并没有使用鲁班猫上的npu。所以我尝试了用npu跑yolov5在debain10的环境下用python接口效果如下：

Python与c++接口将图片监测改成实时摄像头的代码：只需更改cv.Capture()函数的摄像头设备号即可。
链接：https://pan.baidu.com/s/1gauOezF-X8ZuvU4b0I4v4A?pwd=jhzs提取码：jhzs

Python接口的yolov7只需更改yolov5代码的锚点即可,以下只列出主函数主要部分，完整的在链接里。

1.   capture = cv2.VideoCapture(9)
   
2.     ref, frame = capture.read()
   
3.     if not ref:
   
4.         raise ValueError("error reading")
   
5. 
   
6.     fps = 0.0
   
7.     while(True):
   
8.         t1 = time.time()
   
9.         #
   
10.         ref, frame = capture.read()
    
11.         if not ref:
    
12.             break
    
13.         # BGRtoRGB
    
14.         frame = cv2.cvtColor(frame,cv2.COLOR_BGR2RGB)
    
15.         
    
16.         #############
    
17.         #
    
18.         img = frame
    
19.         img, ratio, (dw, dh) = letterbox(img, new_shape=(IMG_SIZE, IMG_SIZE))
    
20.         img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
    
21. 
    
22.         # Inference
    
23.         print('--> Running model')
    
24.         outputs = rknn.inference(inputs=[img])
    
25.         
    
26.    
    
27.         input0_data = outputs[0]
    
28.         input1_data = outputs[1]
    
29.         input2_data = outputs[2]
    
30. 
    
31.         input0_data = input0_data.reshape([3, -1]+list(input0_data.shape[-2:]))
    
32.         input1_data = input1_data.reshape([3, -1]+list(input1_data.shape[-2:]))
    
33.         input2_data = input2_data.reshape([3, -1]+list(input2_data.shape[-2:]))
    
34. 
    
35.         input_data = list()
    
36.         input_data.append(np.transpose(input0_data, (2, 3, 0, 1)))
    
37.         input_data.append(np.transpose(input1_data, (2, 3, 0, 1)))
    
38.         input_data.append(np.transpose(input2_data, (2, 3, 0, 1)))
    
39. 
    
40.         boxes, classes, scores = yolov5_post_process(input_data)
    
41. 
    
42. 
    
43.         img_1 = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
    
44.         #img_1 = img_1[:,:,::-1]
    
45.         if boxes is not None:
    
46.             draw(img_1, boxes, scores, classes)
    
47.         fps  = ( fps + (1./(time.time()-t1)) ) / 2
    
48.         print("fps= %.2f"%(fps))
    
49.         #img_1 = cv2.putText(frame, "fps= %.2f"%(fps), (0, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
    
50.         cv2.imshow("video",img_1[:,:,::-1])
    
51.         c= cv2.waitKey(1) & 0xff
    
52.         if c==27:
    
53.             capture.release()
    
54.             break
    
55.     print("Video Detection Done!")
    
56.     capture.release()
    
57.     cv2.destroyAllWindows()

复制代码

但还没用接到ros中，为此我去翻rknn的github找到了接入ros的方法。

Ros功能包如下：Launch文件：

yolov5.launch

1. <launch>
   
2. 
   
3.   <arg name="display_output" default="true"/>
   
4.   <arg name="camera_topic" default="/usb_cam/image_raw"/>
   
5.   <arg name="chip_type" default="RK3566"/>
   
6. 
   
7.   <node name="rknn_yolov5_node" pkg="rknn_ros" type="rknn_yolov5_node" output="screen">
   
8.     <param name="model_file" value="yolov5s-640-640.rknn"/>
   
9.     <param name="display_output" value="$(arg display_output)"/>
   
10.     <param name="prob_threshold" value="0.35"/>
    
11.     <param name="chip_type" value="$(arg chip_type)"/>
    
12.     <remap from="/camera/image_raw" to="$(arg camera_topic)"/>
    
13.   </node>
    
14. </launch>

复制代码

Camrea.Launch

1. <launch>
   
2.   <arg name="device" default="video9"/>
   
3.   <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" >
   
4.       <param name="video_device" value="/dev/$(arg device)" />
   
5.       <param name="image_width" value="640" />
   
6.       <param name="image_height" value="480" />
   
7.       <param name="framerate" value="30" />
   
8.       <param name="pixel_format" value="yuyv" />
   
9.       <param name="camera_frame_id" value="usn_cam" />
   
10.       <param name="io_method" value="mmap"/>
    
11.       <param name="camera_name" value="usn_cam"/>
    
12.   </node>
    
13. </launch>

复制代码

启动摄像头默认的摄像头设备号为video0 鲁班猫为video9  

1、roslaunch rknn_ros camera.launch

2、roslaunch rknn_ros camera.launch device:=video9（可传参或者改launch）

3、roslaunch rknn_ros yolov5.launch chip_type:=RK3566
（ros包链接）

链接：https://pan.baidu.com/s/1QhfRjDs1sftAB0Q-TS5dBA?pwd=jhzs提取码：jhzs 不出意外改好板子型号和对应的video就能用了。可打开rviz或者rqt_image_view查看。

模型是我自己训练的，链接如下：链接：https://pan.baidu.com/s/1FSJyW6kp4cy3-yakTq_Q4g?pwd=jhzs提取码：jhzs

YOLOV5配置和使用：官方的源码是不建议的：用这个：https://gitcode.net/mirrors/airockchip/yolov5?utm_source=csdn_github_accelerator这是瑞芯微官方推荐的源码，但是也需要更改。

yolov5-master\models下的yolo.py找到def forward(self, x):函数，更改为：

1. def forward(self, x):
   
2.             z = []  # inference output
   
3.             for i in range(self.nl):
   
4.                 if os.getenv('RKNN_model_hack', '0') != '0':
   
5.                     z.append(torch.sigmoid(self.m[i](x[i])))
   
6.                     continue
   
7.    
   
8.                 x[i] = self.m[i](x[i])  # conv
   
9.                 '''
   
10.                 bs, _, ny, nx = x[i].shape  # x(bs,255,20,20) to x(bs,3,20,20,85)
    
11.                 x[i] = x[i].view(bs, self.na, self.no, ny, nx).permute(0, 1, 3, 4, 2).contiguous()
    
12.    
    
13.                 if not self.training:  # inference
    
14.                     if self.onnx_dynamic or self.grid[i].shape[2:4] != x[i].shape[2:4]:
    
15.                         self.grid[i], self.anchor_grid[i] = self._make_grid(nx, ny, i)
    
16.    
    
17.                     y = x[i].sigmoid()
    
18.                     if self.inplace:
    
19.                         y[..., 0:2] = (y[..., 0:2] * 2 + self.grid[i]) * self.stride[i]  # xy
    
20.                         y[..., 2:4] = (y[..., 2:4] * 2) ** 2 * self.anchor_grid[i]  # wh
    
21.                     else:  # for YOLOv5 on AWS Inferentia https://github.com/ultralytics/yolov5/pull/2953
    
22.                         xy, wh, conf = y.split((2, 2, self.nc + 1), 4)  # y.tensor_split((2, 4, 5), 4)  # torch 1.8.0
    
23.                         xy = (xy * 2 + self.grid[i]) * self.stride[i]  # xy
    
24.                         wh = (wh * 2) ** 2 * self.anchor_grid[i]  # wh
    
25.                         y = torch.cat((xy, wh, conf), 4)
    
26.                     z.append(y.view(bs, -1, self.no))
    
27.    
    
28.             if os.getenv('RKNN_model_hack', '0') != '0':
    
29.                 return z
    
30.    
    
31.             return x if self.training else (torch.cat(z, 1),) if self.export else (torch.cat(z, 1), x)
    
32.             '''
    
33.             return x[0],x[1],x[2]

复制代码

这样就可以在pt权重转onnx时去掉最后一个Detect层。

pt转onnx指令python export.py --weights yolov5s.pt --img 640 --batch 1 --opset 11 --include onnx

红色字体部分换成要转换的权重文件例如我的就是：python export.py --weights weights/best.pt --img 640 --batch 1 --opset 11 --include onnxopset选择11。

这样出来的模型是有三个节点的模型，才是可用的。

可用netron查看：netron：https://netron.app/（浏览器网址）将模型拖到页面可查看。

有三个节点。且要记好三个节点的名字。

改好的yolov5：链接：https://pan.baidu.com/s/1fXKNoXhu4m1SmTr4fc-afg?pwd=jhzs提取码：jhzs

Chatgpt部分是b站机器人阿杰github开源项目。https://www.bilibili.com/video/BV12M4y1R76M/?spm_id_from=333.788

效果如图：

整车的sw模型链接：soildwork2020及以上版本可直接打开
链接：https://pan.baidu.com/s/1KqB1SOD418dCvyDaZFMgpg?pwd=jhzs
提取码：jhzs

当时还理想化的撸了个履带，可后来发现打印出来根本用不了，故放弃，换成了轮子。放链接是希望能够帮到像我一样步步踩坑的菜鸟级选手。我是老踩坑怪了。
有不当的地方，还望大佬们海涵。

Cheng0717

大佬厉害啊