🔒 🤰🏻 📮 ROS: mapa de profundidade no Raspberry Pi "baixo nível de sangue" 💪🏼 🕙 👖

Se você usa o ROS ao criar robôs, provavelmente sabe que ele tem suporte para trabalhar com câmeras estéreo. Você pode criar, por exemplo, um mapa de profundidade da parte visível do espaço ou uma nuvem de pontos. E me perguntei como seria fácil usar uma câmera estéreo StereoPi baseada em framboesa no ROS. Antes, eu já estava convencido de que o mapa de profundidade foi perfeitamente construído pelo OpenCV, mas nunca lidei com o ROS. E eu decidi tentar. Quero falar sobre minhas aventuras para encontrar uma solução.

1. Existe algum ROS no Raspberry Pi?

No começo, decidi descobrir se era possível criar ROS para o Raspberry Pi. A primeira coisa que o Google me disse foi uma lista de instruções para instalar versões diferentes do ROS no Raspberry Pi, a saber, esta página wiki do ROS

Bem, já existe algo para começar! Lembrei-me bem de quanto tempo levava para criar o OpenCV no Raspberry (cerca de oito horas), então decidi procurar imagens prontas de cartões MicroSD para economizar tempo.

2. Existem imagens de cartão microSD prontas com ROS para Raspberry?

Aconteceu que esse problema já foi resolvido por várias equipes de desenvolvimento. Se você não cria versões únicas de entusiastas, destacam-se algumas imagens que são constantemente atualizadas com o lançamento de novas versões do sistema operacional e do ROS.

A primeira opção é o ROS instalado no sistema operacional Raspbian nativo da equipe ROSbots. Aqui está uma página com um link de imagem atualizado: pronto para usar a imagem raspbian-stretch-ros-opencv

A segunda são as imagens da Ubiquiti Robotics no ubuntu .

Bem, a segunda pergunta também foi rápida o suficiente. É hora de mergulhar mais fundo.

3. Como o ROS funciona com a câmera Raspberry Pi?

E quais câmeras estéreo geralmente são suportadas no ROS? Olhei a página com câmeras estéreo, para as quais foi declarada a disponibilidade de drivers prontos para o ROS, este:

wiki.ros.org/Sensors

Havia duas seções:
2.3 Sensores 3D (telêmetros e câmeras RGB-D)
2.5 Câmeras
Descobriu-se que, na primeira seção, não apenas as câmeras estéreo, mas também os sensores TOF e os lidares de digitalização estão listados - em geral, tudo o que pode fornecer informações imediatamente em 3D. E no segundo já existem câmeras estéreo. Tentar ver os drivers de várias câmeras estéreo não contribuiu para a minha alegria, pois sugeria uma imersão séria no código.

Ok, dê um passo atrás. Como funciona com uma única câmera Raspberry Pi no ROS?

Três surpresas agradáveis me aguardavam aqui:

acontece que, para o ROS, existe um nó raspicam_node especial apenas para trabalhar com a câmera Raspberry Pi
tipos de nó estão no github, o código é mantido ativamente e bem documentado: github.com/UbiquityRobotics/raspicam_node
O autor do nó Rohan Agrawal ( @Rohbotics ) trabalha para uma empresa que oferece suporte ativo a uma das imagens prontas para o Raspberry Pi

Eu olhei para o repositório github raspicam_node e olhei para problemas. Lá encontrei um problema em aberto com o amplo nome "modo estéreo" quase sete meses atrás, sem respostas e comentários. Na verdade, todos os eventos se desenvolveram mais.

4. Hardcore ou não?

Para não fazer perguntas às crianças aos autores, decidi examinar o código-fonte e avaliar o que ameaça a adição do modo estéreo. Eu estava mais interessado na parte do sistema aqui: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src
Bem, os caras escreveram o motorista mergulhando no nível MMAL. Também me lembrei de que o código fonte das framboesas no modo estéreo também está aberto (a evolução pode ser rastreada aqui no fórum da framboesa ), e a tarefa de escrever um driver estéreo completo é solucionável, mas em grande escala. Observando a descrição dos drivers de outras câmeras, percebi que era necessário publicar não apenas as fotos esquerda e direita, mas também fornecer os parâmetros de ambas as câmeras, aplicar resultados de calibração a cada uma e fazer muitas outras coisas. Isso atraiu experimentos por um ou dois meses. Portanto, decidi paralelizar a abordagem, a saber: escreva ao autor uma pergunta sobre o suporte estéreo e procure uma solução mais simples, mas funcional.

5. Diálogos com o autor

No tópico sobre o modo estéreo no github, fiz uma pergunta ao autor, mencionando que o estéreo é suportado por framboesas desde 2014 e sugeri, se necessário, enviar a ele um quadro de depuração para experimentos. Deixe-me lembrá-lo de que eu ainda duvidava que nesta distribuição o aparelho de som funcione como no Raspbian nativo.

Rohan respondeu surpreendentemente rápido, dizendo que o distrito deles usa um núcleo de framboesa e tudo deve funcionar. E pediu para verificar em uma de suas assembléias.

Núcleo de framboesa! Viva! Teoricamente, uma imagem estéreo deve ser capturada sem dançar com um pandeiro!

Baixei e implantei a imagem mais recente usando um link de Rohan e executei um script python simples para capturar um par estéreo. Funcionou!

imagem

Depois disso, Rohan escreveu que procuraria o código do driver para o modo estéreo e escreveu algumas perguntas. Por exemplo, nosso modo estéreo produz uma imagem colada e precisamos cortá-la em duas - esquerda e direita. E a segunda pergunta sobre os parâmetros de calibração de cada câmera é como lidar com isso.

Eu disse que, como primeiro passo, você pode tirar fotos de câmeras de forma independente. Sim, eles não serão sincronizados no tempo de captura e nas configurações (como brilho, contraste e balanço de branco), mas, como primeiro passo, isso pode diminuir.

Rohan prontamente lançou um patch que permite especificar diretamente do ROS de qual câmera tirar fotos. Eu verifiquei - escolher uma câmera funciona, já é um excelente resultado.

6. Ajuda inesperada

E, em seguida, um comentário do usuário Wezzoid aparece no tópico. Ele disse que estava fazendo um projeto baseado em uma câmera estéreo em um Pi Compute 3 usando devboards de framboesa. Seu robô ambulante de quatro patas rastreou a posição do objeto no espaço, mudou a posição das câmeras e manteve a distância especificada (o projeto está publicado em hackaday.io aqui ).

imagem

E ele compartilhou o código no qual ele pegou a imagem, cortou ao meio com python e compartilhou como nós das câmeras esquerda e direita.
Python não é um amigo muito rápido nesses assuntos, então ele usou uma baixa resolução de 320x240 e um truque de boa vida. Se capturarmos uma imagem estéreo lado a lado (uma câmera à esquerda da imagem estéreo e a segunda à direita), o python deve cortar cada uma das 240 linhas pela metade. Mas se você tirar uma foto de cima para baixo (a câmera esquerda é a metade superior do quadro, a direita é a parte inferior), o python corta o array ao meio em uma operação. O que foi feito com sucesso pelo usuário Wezzoid.
Além disso, ele postou seu código python no Pastebin, que fez essa operação. Aqui está:

Código Wezzoid para publicar nós de duas câmeras de um par estéreo

#!/usr/bin/env python # picamera stereo ROS node using dual CSI Pi CS3 board # Wes Freeman 2018 # modified from code by Adrian Rosebrock, pyimagesearch.com # and jensenb, https://gist.github.com/jensenb/7303362 from picamera.array import PiRGBArray from picamera import PiCamera import time import rospy from sensor_msgs.msg import CameraInfo, Image import yaml import io import signal # for ctrl-C handling import sys def parse_calibration_yaml(calib_file): with file(calib_file, 'r') as f: params = yaml.load(f) cam_info = CameraInfo() cam_info.height = params['image_height'] cam_info.width = params['image_width'] cam_info.distortion_model = params['distortion_model'] cam_info.K = params['camera_matrix']['data'] cam_info.D = params['distortion_coefficients']['data'] cam_info.R = params['rectification_matrix']['data'] cam_info.P = params['projection_matrix']['data'] return cam_info # cam resolution res_x = 320 #320 # per camera res_y = 240 #240 target_FPS = 15 # initialize the camera print "Init camera..." camera = PiCamera(stereo_mode = 'top-bottom',stereo_decimate=False) camera.resolution = (res_x, res_y*2) # top-bottom stereo camera.framerate = target_FPS # using several camera options can cause instability, hangs after a while camera.exposure_mode = 'antishake' #camera.video_stabilization = True # fussy about res? stream = io.BytesIO() # ---------------------------------------------------------- #setup the publishers print "init publishers" # queue_size should be roughly equal to FPS or that causes lag? left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1) right_img_pub = rospy.Publisher('right/image_raw', Image, queue_size=1) left_cam_pub = rospy.Publisher('left/camera_info', CameraInfo, queue_size=1) right_cam_pub = rospy.Publisher('right/camera_info', CameraInfo, queue_size=1) rospy.init_node('stereo_pub') # init messages left_img_msg = Image() left_img_msg.height = res_y left_img_msg.width = res_x left_img_msg.step = res_x*3 # bytes per row: pixels * channels * bytes per channel (1 normally) left_img_msg.encoding = 'rgb8' left_img_msg.header.frame_id = 'stereo_camera' # TF frame right_img_msg = Image() right_img_msg.height = res_y right_img_msg.width = res_x right_img_msg.step = res_x*3 right_img_msg.encoding = 'rgb8' right_img_msg.header.frame_id = 'stereo_camera' imageBytes = res_x*res_y*3 # parse the left and right camera calibration yaml files left_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/left.yaml') right_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/right.yaml') # --------------------------------------------------------------- # this is supposed to shut down gracefully on CTRL-C but doesn't quite work: def signal_handler(signal, frame): print 'CTRL-C caught' print 'closing camera' camera.close() time.sleep(1) print 'camera closed' sys.exit(0) signal.signal(signal.SIGINT, signal_handler) #----------------------------------------------------------- print "Setup done, entering main loop" framecount=0 frametimer=time.time() toggle = True # capture frames from the camera for frame in camera.capture_continuous(stream, format="rgb", use_video_port=True): framecount +=1 stamp = rospy.Time.now() left_img_msg.header.stamp = stamp right_img_msg.header.stamp = stamp left_cam_info.header.stamp = stamp right_cam_info.header.stamp = stamp left_cam_pub.publish(left_cam_info) right_cam_pub.publish(right_cam_info) frameBytes = stream.getvalue() left_img_msg.data = frameBytes[:imageBytes] right_img_msg.data = frameBytes[imageBytes:] #publish the image pair left_img_pub.publish(left_img_msg) right_img_pub.publish(right_img_msg) # console info if time.time() > frametimer +1.0: if toggle: indicator = ' o' # just so it's obviously alive if values aren't changing else: indicator = ' -' toggle = not toggle print 'approx publish rate:', framecount, 'target FPS:', target_FPS, indicator frametimer=time.time() framecount=0 # clear the stream ready for next frame stream.truncate(0) stream.seek(0)

7. Comece a publicar os nós das câmeras esquerda e direita

No primeiro início, o código amaldiçoou que não havia acesso aos arquivos YML com os parâmetros da câmera. Usei câmeras V2 cor de framboesa e lembrei que arquivos prontos com resultados de calibração para diferentes modelos de câmera chegaram ao raspicam_node no github: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info
Peguei um deles, fiz duas cópias e salvei com os nomes left.yml e right.yml, escrevendo neles a resolução da câmera a partir do script do autor. Aqui está o que aconteceu com a câmera esquerda:

left.yml

 image_width: 320 image_height: 240 camera_name: left camera_matrix: rows: 3 cols: 3 data: [1276.704618338571, 0, 634.8876509199106, 0, 1274.342831275509, 379.8318028940378, 0, 0, 1] distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 data: [0.1465167016954302, -0.2847343180128725, 0.00134017721235817, -0.004309553450829512, 0] rectification_matrix: rows: 3 cols: 3 data: [1, 0, 0, 0, 1, 0, 0, 0, 1] projection_matrix: rows: 3 cols: 4 data: [1300.127197265625, 0, 630.215390285608, 0, 0, 1300.670166015625, 380.1702884455881, 0, 0, 0, 1, 0]

À direita, o nome da câmera é substituído por right e o próprio arquivo é nomeado right.yml. O restante do arquivo é idêntico.

Como não planejava fazer um projeto complexo, não repeti os caminhos longos do autor com subpastas e apenas coloquei os arquivos na raiz da pasta inicial ao lado do script python. O código foi iniciado com sucesso, exibindo mensagens de status no console.

imagem

Resta apenas ver o que foi finalmente publicado por nossas câmeras esquerda e direita. Para fazer isso, lancei rqt_image_view. Os itens / left / image_raw e / right / image_raw apareceram no menu suspenso.Quando os selecionei, vi imagens das câmeras esquerda e direita.

imagem

Bem, essa coisa ganhou! Agora a parte divertida.

8. Olhamos para o mapa das profundezas.

Para visualizar o mapa de profundidade, não propus minha própria abordagem e reparei no manual ROS clássico para definir parâmetros estéreo .
A partir daí, descobri que seria bom publicar os dois nós em um espaço para nome específico, e não na raiz, como o Wezzoid. Como resultado, as antigas linhas de publicação do formulário

 left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)

começou a ficar assim:

 left_img_pub = rospy.Publisher('stereo/right/image_raw', Image, queue_size=1)

Depois disso, lançamos o nó de processamento no modo estéreo stereo_image_proc:

 ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_ige_proc

Bem, também queremos ver o resultado, então iniciamos o observador:

 rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color

E para configurar os parâmetros do mapa de profundidade, execute o utilitário de configuração:

 rosrun rqt_reconfigure rqt_reconfigure

Como resultado, vemos a figura no início do artigo. Aqui está um pouco maior:

imagem

Todos os arquivos que eu publiquei no github: github.com/realizator/StereoPi-ROS-depth-map-test

9. Planos imediatos

Após minha publicação do resultado em uma discussão no github, Rohan escreveu “Legal! Eu preciso pegar meu StereoPi. ” Escrevemos para ele por correio, enviei uma taxa. Espero que, com o hardware de trabalho em suas mãos, seja mais fácil terminar e depurar um driver estéreo completo para ROS e Raspberry.

10. Resumo

Um mapa de profundidade de uma imagem estéreo em framboesas no ROS pode ser obtido de várias maneiras. O caminho escolhido para a verificação rápida não é o mais ideal em termos de desempenho, mas pode ser usado para fins de aplicativo. A beleza de sua simplicidade e a capacidade de iniciar imediatamente experimentos.

Bem, pelo engraçado: depois de receber os resultados, notei que Wezzoid, que propôs sua solução, foi o autor da pergunta sobre a publicação de duas imagens estéreo. Ele se perguntou, ele decidiu.

ROS: mapa de profundidade no Raspberry Pi "baixo nível de sangue"