diff --git a/projeto1/limpeza_catkin.sh b/projeto1/limpeza_catkin.sh new file mode 100644 index 0000000..3752ae1 --- /dev/null +++ b/projeto1/limpeza_catkin.sh @@ -0,0 +1,7 @@ +cd ~/catkin_ws/src +sudo rm -rf DynamixelSDK +sudo rm -rf dynamixel-workbench +sudo rm -rf dynamixel-workbench-msgs +sudo rm -rf turtlebot3_manipulation +sudo rm -rf turtlebot3_manipulation_simulations +sudo rm -rf roboticsgroup_gazebo_plugins diff --git a/revisao/lista_provas_anteriores.md b/revisao/lista_provas_anteriores.md new file mode 100644 index 0000000..8cb7cb9 --- /dev/null +++ b/revisao/lista_provas_anteriores.md @@ -0,0 +1,2 @@ + + diff --git a/ros/insper_garra/urdf/open_manipulator_x.urdf.xacro b/ros/insper_garra/urdf/open_manipulator_x.urdf.xacro index 8c1a39c..456aa5a 100644 --- a/ros/insper_garra/urdf/open_manipulator_x.urdf.xacro +++ b/ros/insper_garra/urdf/open_manipulator_x.urdf.xacro @@ -192,6 +192,17 @@ + + + + 100 + true + 0.1 + 0.1 + + + + @@ -227,6 +238,17 @@ + + + + 100 + true + 0.1 + 0.1 + + + + diff --git a/ros/projeto1_base/scripts/base_proj.py b/ros/projeto1_base/scripts/base_proj.py index 06993b2..65cd8ac 100755 --- a/ros/projeto1_base/scripts/base_proj.py +++ b/ros/projeto1_base/scripts/base_proj.py @@ -55,42 +55,42 @@ def recebe(msg): - global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado - global y - global z - global id - for marker in msg.markers: - id = marker.id - marcador = "ar_marker_" + str(id) - - print(tf_buffer.can_transform(frame, marcador, rospy.Time(0))) - header = Header(frame_id=marcador) - # Procura a transformacao em sistema de coordenadas entre a base do robo e o marcador numero 100 - # Note que para seu projeto 1 voce nao vai precisar de nada que tem abaixo, a - # Nao ser que queira levar angulos em conta - trans = tf_buffer.lookup_transform(frame, marcador, rospy.Time(0)) - - # Separa as translacoes das rotacoes - x = trans.transform.translation.x - y = trans.transform.translation.y - z = trans.transform.translation.z - # ATENCAO: tudo o que vem a seguir e' so para calcular um angulo - # Para medirmos o angulo entre marcador e robo vamos projetar o eixo Z do marcador (perpendicular) - # no eixo X do robo (que e' a direcao para a frente) - t = transformations.translation_matrix([x, y, z]) - # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions - r = transformations.quaternion_matrix([trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w]) - m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes - z_marker = [0,0,1,0] # Sao 4 coordenadas porque e' um vetor em coordenadas homogeneas - v2 = numpy.dot(m, z_marker) - v2_n = v2[0:-1] # Descartamos a ultima posicao - n2 = v2_n/linalg.norm(v2_n) # Normalizamos o vetor - x_robo = [1,0,0] - cosa = numpy.dot(n2, x_robo) # Projecao do vetor normal ao marcador no x do robo - angulo_marcador_robo = math.degrees(math.acos(cosa)) - - # Terminamos - print("id: {} x {} y {} z {} angulo {} ".format(id, x,y,z, angulo_marcador_robo)) + global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado + global y + global z + global id + for marker in msg.markers: + id = marker.id + marcador = "ar_marker_" + str(id) + + print(tf_buffer.can_transform(frame, marcador, rospy.Time(0))) + header = Header(frame_id=marcador) + # Procura a transformacao em sistema de coordenadas entre a base do robo e o marcador numero 100 + # Note que para seu projeto 1 voce nao vai precisar de nada que tem abaixo, a + # Nao ser que queira levar angulos em conta + trans = tf_buffer.lookup_transform(frame, marcador, rospy.Time(0)) + + # Separa as translacoes das rotacoes + x = trans.transform.translation.x + y = trans.transform.translation.y + z = trans.transform.translation.z + # ATENCAO: tudo o que vem a seguir e' so para calcular um angulo + # Para medirmos o angulo entre marcador e robo vamos projetar o eixo Z do marcador (perpendicular) + # no eixo X do robo (que e' a direcao para a frente) + t = transformations.translation_matrix([x, y, z]) + # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions + r = transformations.quaternion_matrix([trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w]) + m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes + z_marker = [0,0,1,0] # Sao 4 coordenadas porque e' um vetor em coordenadas homogeneas + v2 = numpy.dot(m, z_marker) + v2_n = v2[0:-1] # Descartamos a ultima posicao + n2 = v2_n/linalg.norm(v2_n) # Normalizamos o vetor + x_robo = [1,0,0] + cosa = numpy.dot(n2, x_robo) # Projecao do vetor normal ao marcador no x do robo + angulo_marcador_robo = math.degrees(math.acos(cosa)) + + # Terminamos + print("id: {} x {} y {} z {} angulo {} ".format(id, x,y,z, angulo_marcador_robo)) diff --git a/ros/projeto1_base/scripts/bug_visao_module.ipynb b/ros/projeto1_base/scripts/bug_visao_module.ipynb new file mode 100644 index 0000000..e3fa406 --- /dev/null +++ b/ros/projeto1_base/scripts/bug_visao_module.ipynb @@ -0,0 +1,204 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Bug da função identifica cor em `visao_module.py`\n", + "\n", + "## Teste com a função original\n", + "Inicialmente, fazemos o teste com a função original\n" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [], + "source": [ + "import cv2\n", + "import numpy as np\n", + "\n", + "def identifica_cor(frame):\n", + " '''\n", + " Segmenta o maior objeto cuja cor é parecida com cor_h (HUE da cor, no espaço HSV).\n", + " '''\n", + "\n", + " # No OpenCV, o canal H vai de 0 até 179, logo cores similares ao\n", + " # vermelho puro (H=0) estão entre H=-8 e H=8.\n", + " # Precisamos dividir o inRange em duas partes para fazer a detecção\n", + " # do vermelho:\n", + " frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)\n", + "\n", + " cor_menor = np.array([80, 50, 50])\n", + " cor_maior = np.array([120, 255, 255])\n", + " segmentado_cor = cv2.inRange(frame_hsv, cor_menor, cor_maior)\n", + "\n", + " #cor_menor = np.array([85, 50, 50])\n", + " #cor_maior = np.array([115, 255, 255])\n", + " #segmentado_cor += cv2.inRange(frame_hsv, cor_menor, cor_maior)\n", + "\n", + " # Note que a notacão do numpy encara as imagens como matriz, portanto o enderecamento é\n", + " # linha, coluna ou (y,x)\n", + " # Por isso na hora de montar a tupla com o centro precisamos inverter, porque\n", + " centro = (frame.shape[1]//2, frame.shape[0]//2)\n", + "\n", + "\n", + " def cross(img_rgb, point, color, width,length):\n", + " cv2.line(img_rgb, (point[0] - length/2, point[1]), (point[0] + length/2, point[1]), color ,width, length)\n", + " cv2.line(img_rgb, (point[0], point[1] - length/2), (point[0], point[1] + length/2),color ,width, length)\n", + "\n", + "\n", + "\n", + " # A operação MORPH_CLOSE fecha todos os buracos na máscara menores\n", + " # que um quadrado 7x7. É muito útil para juntar vários\n", + " # pequenos contornos muito próximos em um só.\n", + " segmentado_cor = cv2.morphologyEx(segmentado_cor,cv2.MORPH_CLOSE,np.ones((7, 7)))\n", + "\n", + " # Encontramos os contornos na máscara e selecionamos o de maior área\n", + " #contornos, arvore = cv2.findContours(segmentado_cor.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)\n", + " contornos, arvore = cv2.findContours(segmentado_cor.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)\n", + "\n", + " maior_contorno = None\n", + " maior_contorno_area = 0\n", + "\n", + " for cnt in contornos:\n", + " area = cv2.contourArea(cnt)\n", + " if area > maior_contorno_area:\n", + " maior_contorno = cnt\n", + " maior_contorno_area = area\n", + "\n", + " # Encontramos o centro do contorno fazendo a média de todos seus pontos.\n", + " if not maior_contorno is None :\n", + " cv2.drawContours(frame, [maior_contorno], -1, [0, 0, 255], 5)\n", + " maior_contorno = np.reshape(maior_contorno, (maior_contorno.shape[0], 2))\n", + " media = maior_contorno.mean(axis=0)\n", + " media = media.astype(np.int32)\n", + " cv2.circle(frame, (media[0], media[1]), 5, [0, 255, 0])\n", + " cross(frame, centro, [255,0,0], 1, 17)\n", + " else:\n", + " media = (0, 0)\n", + "\n", + " # Representa a area e o centro do maior contorno no frame\n", + " font = cv2.FONT_HERSHEY_COMPLEX_SMALL\n", + " cv2.putText(frame,\"{:d} {:d}\".format(*media),(20,100), 1, 4,(255,255,255),2,cv2.LINE_AA)\n", + " cv2.putText(frame,\"{:0.1f}\".format(maior_contorno_area),(20,50), 1, 4,(255,255,255),2,cv2.LINE_AA)\n", + "\n", + " # was: return centro, result_frame, result_tuples\n", + " return centro, media, area, frame" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Imagem 1: marcador pequeno está dentro do contorno do creeper azul\n", + "\n", + "![](creeper_azul_marcador_in.png)" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "output_type": "display_data", + "data": { + "image/png": "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\n", + "text/plain": "" + }, + "metadata": {} + }, + { + "output_type": "stream", + "name": "stdout", + "text": "Area: 383.5\n" + } + ], + "source": [ + "img_marcador_in = cv2.imread(\"creeper_azul_marcador_in.png\")\n", + "\n", + "centro, media, area, frame = identifica_cor(img_marcador_in)\n", + "\n", + "from __future__ import print_function\n", + "%matplotlib inline\n", + "import matplotlib.pyplot as plt\n", + "\n", + "plt.imshow(frame[:,:,::-1])\n", + "plt.xticks([]); plt.yticks([])\n", + "plt.show()\n", + "\n", + "print(\"Area: \", area)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Imagem 2: marcador pequeno está fora do contorno do creeper azul\n", + "\n", + "![](creeper_azul_marcador_out.png)" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [ + { + "output_type": "display_data", + "data": { + "image/png": "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\n", + "text/plain": "" + }, + "metadata": {} + }, + { + "output_type": "stream", + "name": "stdout", + "text": "Area: 7490.0\n" + } + ], + "source": [ + "img_marcador_out = cv2.imread(\"creeper_azul_marcador_out.png\")\n", + "\n", + "centro, media, area, frame = identifica_cor(img_marcador_out)\n", + "\n", + "plt.imshow(frame[:,:,::-1])\n", + "plt.xticks([]); plt.yticks([])\n", + "plt.show()\n", + "\n", + "print(\"Area: \", area)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 2 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython2", + "version": "2.7.17-final" + }, + "orig_nbformat": 2, + "kernelspec": { + "name": "python271764bita7620562d5304f68a8300b5bf6d8ab9d", + "display_name": "Python 2.7.17 64-bit" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} \ No newline at end of file diff --git a/ros/projeto1_base/scripts/creeper_azul_marcador_in.png b/ros/projeto1_base/scripts/creeper_azul_marcador_in.png new file mode 100644 index 0000000..0d0bd32 Binary files /dev/null and b/ros/projeto1_base/scripts/creeper_azul_marcador_in.png differ diff --git a/ros/projeto1_base/scripts/creeper_azul_marcador_out.png b/ros/projeto1_base/scripts/creeper_azul_marcador_out.png new file mode 100644 index 0000000..ee5cd3b Binary files /dev/null and b/ros/projeto1_base/scripts/creeper_azul_marcador_out.png differ diff --git a/ros/projeto1_base/scripts/exemplos_transformacoes.py b/ros/projeto1_base/scripts/exemplos_transformacoes.py new file mode 100755 index 0000000..0e9f51c --- /dev/null +++ b/ros/projeto1_base/scripts/exemplos_transformacoes.py @@ -0,0 +1,518 @@ +#! /usr/bin/env python +# -*- coding:utf-8 -*- + +from __future__ import print_function, division +import rospy +import numpy as np +import numpy +import tf +import math +import cv2 +import time +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Image, CompressedImage +from cv_bridge import CvBridge, CvBridgeError +from numpy import linalg +from tf import transformations +from tf import TransformerROS +import tf2_ros +from geometry_msgs.msg import Twist, Vector3, Pose, Vector3Stamped +from ar_track_alvar_msgs.msg import AlvarMarker, AlvarMarkers +from nav_msgs.msg import Odometry +from std_msgs.msg import Header + + +import visao_module + + +bridge = CvBridge() + +cv_image = None +media = [] +centro = [] + +ids_possiveis_tags = [11,12,13,21,22,23] # Baseado no enunciado do projeto + +area = 0.0 # Variavel com a area do maior contorno + +# Só usar se os relógios ROS da Raspberry e do Linux desktop estiverem sincronizados. +# Descarta imagens que chegam atrasadas demais +check_delay = False + +resultados = [] # Criacao de uma variavel global para guardar os resultados vistos + +x = 0 +y = 0 +z = 0 +id = 0 + +frame = "camera_link" +# frame = "head_camera" # DESCOMENTE para usar com webcam USB via roslaunch tag_tracking usbcam + +tfl = 0 + +tf_buffer = tf2_ros.Buffer() + + +def faz_transformacao(ref1, ref2): + """Realiza a transformacao do ponto entre o referencial 1 e o referencial 2 + retorna a trasnformacao + """ + print(tf_buffer.can_transform(ref1, ref2, rospy.Time(0))) + transf = tf_buffer.lookup_transform(ref1, ref2, rospy.Time(0)) + return transf + +def decompoe(transf): + """Recebe uma transformacao de sistemas de coordenadas e a converte em x,y,z e ângulo em RAD em relação a z""" + # Separa as translacoes das rotacoes + x = transf.transform.translation.x + y = transf.transform.translation.y + z = transf.transform.translation.z + # ATENCAO: tudo o que vem a seguir e' so para calcular um angulo + # Para medirmos o angulo entre marcador e robo vamos projetar o eixo Z do marcador (perpendicular) + # no eixo X do robo (que e' a direcao para a frente) + t = transformations.translation_matrix([x, y, z]) + # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions + r = transformations.quaternion_matrix([transf.transform.rotation.x, transf.transform.rotation.y, transf.transform.rotation.z, transf.transform.rotation.w]) + m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes + z_marker = [0,0,1,0] # Sao 4 coordenadas porque e' um vetor em coordenadas homogeneas + v2 = numpy.dot(m, z_marker) + v2_n = v2[0:-1] # Descartamos a ultima posicao + n2 = v2_n/linalg.norm(v2_n) # Normalizamos o vetor + x_robo = [1,0,0] + cosa = numpy.dot(n2, x_robo) # Projecao do vetor normal ao marcador no x do robo + angulo_marcador_robo = math.acos(cosa) + return x,y,z, angulo_marcador_robo + +def insere_coords_dict(dici, x,y,z,alpha): + dici["x"] = x + dici["y"] = y + dici["z"] = z + dici["alpha"] = alpha + dici["graus"] = math.degrees(alpha) + + +def recebe(msg): + global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado + global y + global z + global id + + + frame_names = {"camera_rgb_optical_frame":"Coordenadas da câmera", "end_effector_link": "Cubo vermelho da mão", "base_link": "Base do robô"} + frame_coords = {"camera_rgb_optical_frame":dict(), "end_effector_link": dict(), "base_link":dict()} + + + for marker in msg.markers: + id = marker.id + marcador = "ar_marker_" + str(id) + + referenciais = frame_names.keys() + for ref in referenciais: + transf = faz_transformacao(marcador, ref) + if transf is not None: + xt, yt, zt, alpha_t = decompoe(transf) + insere_coords_dict(frame_coords[ref], xt, yt, zt, alpha_t) + + for ref in frame_names.keys(): + print("\r") + if ref in frame_coords.keys(): + print("Marcador ", id) + print("No referencial Referencial :",ref, " que é ", end=None) + print(frame_names[ref]) + for k in frame_coords[ref].keys(): + print("%s %5.3f"%(k, frame_coords[ref][k])) + + +# A função a seguir é chamada sempre que chega um novo frame +def roda_todo_frame(imagem): + # print("frame") + global cv_image + global media + global centro + global resultados + + try: + antes = time.clock() + temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8") + # Note que os resultados já são guardados automaticamente na variável + # chamada resultados + centro, saida_net, resultados = visao_module.processa(temp_image) + for r in resultados: + # print(r) - print feito para documentar e entender + # o resultado + pass + depois = time.clock() + # Desnecessário - Hough e MobileNet já abrem janelas + cv_image = saida_net.copy() + except CvBridgeError as e: + print('ex', e) + +if __name__=="__main__": + + print(""" + +Para funcionar este programa *precisa* do Rviz rodando antes: + + + +roslaunch turtlebot3_manipulation_moveit_config move_group.launch + +roslaunch my_simulation rviz.launch + +""" + + + ) + + rospy.init_node("cor") + + topico_imagem = "/camera/rgb/image_raw/compressed" + + recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size = 2**24) + recebedor = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, recebe) # Para recebermos notificacoes de que marcadores foram vistos + + + print("Usando ", topico_imagem) + + velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 1) + + tfl = tf2_ros.TransformListener(tf_buffer) #conversao do sistema de coordenadas + tolerancia = 25 + + # Exemplo de categoria de resultados + # [('chair', 86.965459585189819, (90, 141), (177, 265))] + + try: + # Inicializando - por default gira no sentido anti-horário + # vel = Twist(Vector3(0,0,0), Vector3(0,0,math.pi/10.0)) + + while not rospy.is_shutdown(): + for r in resultados: + + pass # Para evitar prints + # print(r) + #velocidade_saida.publish(vel) + + if cv_image is not None: + # Note que o imshow precisa ficar *ou* no codigo de tratamento de eventos *ou* no thread principal, não em ambos + cv2.imshow("cv_image no loop principal", cv_image) + cv2.waitKey(1) + rospy.sleep(0.1) + + except rospy.ROSInterruptException: + print("Ocorreu uma exceção com o rospy") + + +#! /usr/bin/env python +# -*- coding:utf-8 -*- + +from __future__ import print_function, division +import rospy +import numpy as np +import numpy +import tf +import math +import cv2 +import time +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Image, CompressedImage +from cv_bridge import CvBridge, CvBridgeError +from numpy import linalg +from tf import transformations +from tf import TransformerROS +import tf2_ros +from geometry_msgs.msg import Twist, Vector3, Pose, Vector3Stamped +from ar_track_alvar_msgs.msg import AlvarMarker, AlvarMarkers +from nav_msgs.msg import Odometry +from std_msgs.msg import Header + + +import visao_module + + +bridge = CvBridge() + +cv_image = None +media = [] +centro = [] + +ids_possiveis_tags = [11,12,13,21,22,23] # Baseado no enunciado do projeto + +area = 0.0 # Variavel com a area do maior contorno colorido + +resultados = [] # Criacao de uma variavel global para guardar os resultados vistos pela MobileNet + +x = 0 +y = 0 +z = 0 + +tfl = 0 + +tf_buffer = tf2_ros.Buffer() + +marcadores_vistos = [] + +# Matriz de projeçao da camera. Pode ser obtida com rostopic echo /camera/rgb/camera_info +# Veja mais sobre a matematica relevante aqui, se precisar +# http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html +P = np.array([[530.4669406576809, 0.0, 320.5, -37.13268584603767],[0.0, 530.4669406576809, 240.5, 0.0],[0.0, 0.0, 1.0, 0.0]], dtype=float) + +# Os pontos em coordenadas do creeper chegam atraves dos alvar bundles. Só que lá estão em cm e precisam ser convertidos em m +# https://github.com/arnaldojr/my_simulation/blob/master/alvar_bundles/markers_11_azul.xml +tag_big = np.array(([[-0.075,-0.075,0.013,1.0],[0.075,-0.075,0.013,1.0 ],[0.075,0.075,0.013,1.0],[-0.075,0.075,0.013, 1.0]])) +tag_small = np.array([[-0.0149,-0.3423,0.058, 1.0],[0.0149,-0.3423,0.058, 1.0],[0.0149,-0.3111,0.058, 1.0],[-0.0149,-0.3111,0.058, 1.]]) +tags = [tag_big, tag_small] + +def random_color(): + """Retorna uma cor aleatória""" + h = np.random.randint(low=0,high=180) # It's easier to randomize colors in the H component + s = 200 + v = 255 + hsv_img = np.array([[[h,s,v]]], dtype=np.dtype('u1')) # We create a one-pixel image in HSV + #print(hsv_img) + rgb_img = cv2.cvtColor(hsv_img, cv2.COLOR_HSV2RGB) + color_val = rgb_img[0][0].astype(float) + #color_val/=255 + #print(color_val) + return color_val + +def project1(M,P,p): + """Converte um ponto de coordenadas camera_rgb_optical_frame para coordenadas de pixel""" + pcamera = np.dot(M, p) + pscreen = np.dot(P, pcamera) + p_float = np.true_divide(pscreen[:-1], pscreen[-1]) + p_int = np.array(p_float, dtype=np.int32) + return p_int + +def convert_and_draw_tags(M,P, tag_points, image, color, id): + """Usando a matriz M que converte do marker para camera_rgb_optical_frame + e a matriz P imutável da câmera + Desenha o contorno de onde o tag foi detectado + """ + points = [] + mx = 0 + my = 0 + for p in tag_points: + # Dá para melhorar este for e este + # project vetorizando - todos os pontos de 1 vez + p_screen = project1(M,P,p) + mx+=p_screen[0] + my+=p_screen[1] + points.append(list(p_screen)) + sz = len(tag_points) + mx/=sz # daria para melhorar esta conta + my/=sz # se a media fosse feita antes de converter para int + p_array = np.array([points], dtype=np.int32) + cv2.polylines(image, [p_array], True, color,3) + centro_tag = (int(mx), int(my)) + cv2.circle(image, centro_tag,3, color, 3, 8, 0) + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 1 + font_thickness = 2 + cv2.putText(image, str(id), (int(mx)+8, int(my)), font, font_scale, color, font_thickness, cv2.LINE_AA) + + + +def faz_transformacao(ref1, ref2): + """Realiza a transformacao do ponto entre o referencial 1 e o referencial 2 + retorna a trasnformacao + + Para saber todos os referenciais disponíveis veja o frames.pdf gerado por: + + rosrun tf view_frames + """ + possivel_converter = tf_buffer.can_transform(ref1, ref2, rospy.Time(0)) + if possivel_converter: + transf = tf_buffer.lookup_transform(ref1, ref2, rospy.Time(0)) + return transf + else: + return None + +def decompoe(transf): + """Recebe uma transformacao de sistemas de coordenadas e a converte em x,y,z e ângulo em RAD em relação a z""" + # Separa as translacoes das rotacoes + x = transf.transform.translation.x + y = transf.transform.translation.y + z = transf.transform.translation.z + # ATENCAO: tudo o que vem a seguir e' so para calcular um angulo + # Para medirmos o angulo entre marcador e robo vamos projetar o eixo Z do marcador (perpendicular) + # no eixo X do robo (que e' a direcao para a frente) + t = transformations.translation_matrix([x, y, z]) + # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions + r = transformations.quaternion_matrix([transf.transform.rotation.x, transf.transform.rotation.y, transf.transform.rotation.z, transf.transform.rotation.w]) + m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes + z_marker = [0,0,1,0] # Sao 4 coordenadas porque e' um vetor em coordenadas homogeneas + v2 = numpy.dot(m, z_marker) + v2_n = v2[0:-1] # Descartamos a ultima posicao + n2 = v2_n/linalg.norm(v2_n) # Normalizamos o vetor + x_robo = [1,0,0] + cosa = numpy.dot(n2, x_robo) # Projecao do vetor normal ao marcador no x do robo + angulo_marcador_robo = math.acos(cosa) + return x,y,z, angulo_marcador_robo + +def insere_coords_dict(dici, x,y,z,alpha): + " Insere coordenadas (vindas de um transformation) num dicionário para facilitar uso posterior" + dici["x"] = x + dici["y"] = y + dici["z"] = z + dici["alpha"] = alpha + dici["graus"] = math.degrees(alpha) + + +def recebe(msg): + "Recebe o evento das tags alvar" + global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado + global y + global z + global id + + frame_names = {"camera_rgb_optical_frame":"Coordenadas da câmera", "end_effector_link": "Cubo vermelho da mão", "base_link": "Base do robô"} + frame_coords = {"camera_rgb_optical_frame":dict(), "end_effector_link": dict(), "base_link":dict()} + + markers_on_screen = [] + + for marker in msg.markers: + id = marker.id + marcador = "ar_marker_" + str(id) + markers_on_screen.append(id) + + referenciais = frame_names.keys() + for ref in referenciais: + transf = faz_transformacao(ref, marcador) + if transf is not None: + xt, yt, zt, alpha_t = decompoe(transf) + insere_coords_dict(frame_coords[ref], xt, yt, zt, alpha_t) + + for ref in frame_names.keys(): + print("\r") + if ref in frame_coords.keys(): + print("Marcador ", id) + print("No referencial :",ref, " que é ", end=None) + print(frame_names[ref]) + for k in frame_coords[ref].keys(): + print("%s %5.3f"%(k, frame_coords[ref][k]), end=" ") + print() + global marcadores_vistos + marcadores_vistos = markers_on_screen + desenha_tags_tela(marcadores_vistos, cv_image) + + +def desenha_tags_tela(markers_vistos, imagem_bgr): + print("DESENHA ", markers_vistos) + + camera = "camera_rgb_optical_frame" # Se usar a camera da garra troque este nome + for id in markers_vistos: + if id in ids_possiveis_tags: # double check para evitar tags fantasmas + marcador = "ar_marker_" + str(id) + transf = faz_transformacao(camera, marcador) + if transf is not None: + # Monta a matriz de traslacao - isso eh trabalhoso no OS + t = transformations.translation_matrix([transf.transform.translation.x, transf.transform.translation.y, transf.transform.translation.z]) + # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions + r = transformations.quaternion_matrix([transf.transform.rotation.x, transf.transform.rotation.y, transf.transform.rotation.z, transf.transform.rotation.w]) + m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes que converte do frame do marcador para o frame da camera + for t in tags: + convert_and_draw_tags(m,P, t, imagem_bgr, random_color(), id): + + + +def projeta(m, coords): + "Recebe uma matriz translacao/rotacao M e lista de coordenadas homogeneas (com 4 elementos), e os projeta na tela" + return numpy.dot(m, coords) + # saida = [] + # for coord in coords: + # proj = numpy.dot(m, coords) + # saida.append(proj) + + + +# A função a seguir é chamada sempre que chega um novo frame +def roda_todo_frame(imagem): + # print("frame") + global cv_image + global media + global centro + global resultados + + try: + antes = time.clock() + temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8") + # Note que os resultados já são guardados automaticamente na variável + # chamada resultados + centro, saida_net, resultados = visao_module.processa(temp_image) + for r in resultados: + # print(r) - print feito para documentar e entender + # o resultado + pass + depois = time.clock() + # Desnecessário - Hough e MobileNet já abrem janelas + cv_image = saida_net.copy() + except CvBridgeError as e: + print('ex', e) + +if __name__=="__main__": + + print(""" + +ALERTA: + + + + + + + +Para funcionar este programa *precisa* do Rviz rodando antes: + +roslaunch turtlebot3_manipulation_moveit_config move_group.launch + +roslaunch my_simulation rviz.launch + +""" + + + ) + + rospy.init_node("cor") + + topico_imagem = "/camera/rgb/image_raw/compressed" + + recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size = 2**24) + recebedor = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, recebe) # Para recebermos notificacoes de que marcadores foram vistos + + + print("Usando ", topico_imagem) + + velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 1) + + tfl = tf2_ros.TransformListener(tf_buffer) #conversao do sistema de coordenadas + tolerancia = 25 + + # Exemplo de categoria de resultados MobileNEt + # [('chair', 86.965459585189819, (90, 141), (177, 265))] + + try: + # Inicializando - por default gira no sentido anti-horário + # vel = Twist(Vector3(0,0,0), Vector3(0,0,math.pi/10.0)) + + while not rospy.is_shutdown(): + for r in resultados: + + pass # Para evitar prints + # print(r) + #velocidade_saida.publish(vel) + + # marcadores_vistos = [] + + if cv_image is not None: + # Note que o imshow precisa ficar *ou* no codigo de tratamento de eventos *ou* no thread principal, não em ambos + # desenha_tags_tela(marcadores_vistos, cv_image) + cv2.imshow("cv_image no loop principal", cv_image) + cv2.waitKey(1) + rospy.sleep(0.1) + + except rospy.ROSInterruptException: + print("Ocorreu uma exceção com o rospy") + + diff --git a/ros/projeto1_base/scripts/teste_transf.py b/ros/projeto1_base/scripts/teste_transf.py new file mode 100755 index 0000000..82addf8 --- /dev/null +++ b/ros/projeto1_base/scripts/teste_transf.py @@ -0,0 +1,330 @@ +#! /usr/bin/env python +# -*- coding:utf-8 -*- + +from __future__ import print_function, division +import rospy +import numpy as np +import numpy +import tf +import math +import cv2 +import time +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Image, CompressedImage +from cv_bridge import CvBridge, CvBridgeError +from numpy import linalg +from tf import transformations +from tf import TransformerROS +import tf2_ros +from geometry_msgs.msg import Twist, Vector3, Pose, Vector3Stamped +from ar_track_alvar_msgs.msg import AlvarMarker, AlvarMarkers +from nav_msgs.msg import Odometry +from std_msgs.msg import Header + + +import visao_module + + +bridge = CvBridge() + +cv_image = None +media = [] +centro = [] + +ids_possiveis_tags = [11,12,13,21,22,23] # Baseado no enunciado do projeto + +area = 0.0 # Variavel com a area do maior contorno colorido + +resultados = [] # Criacao de uma variavel global para guardar os resultados vistos pela MobileNet + +x = 0 +y = 0 +z = 0 + +ofx = 640 +ofy = 480 + +tfl = 0 + +tf_buffer = tf2_ros.Buffer() + +marcadores_vistos = [] + +# Matriz de projeçao da camera. Pode ser obtida com rostopic echo /camera/rgb/camera_info +# Veja mais sobre a matematica relevante aqui, se precisar +# http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html +#P = np.array([[530.4669406576809, 0.0, 320.5, -37.13268584603767],[0.0, 530.4669406576809, 240.5, 0.0],[0.0, 0.0, 1.0, 0.0]], dtype=float) +#P = np.array([[530.4669406576809, 0.0, 0, -37.13268584603767],[0.0, 530.4669406576809, 240.5, 0.0],[0.0, 0.0, 1.0, 0.0]], dtype=float) +#P = np.array([[-530.4669406576809, 0.0, 0, +37.13268584603767],[0.0, -530.4669406576809, -240.5, 0.0],[0.0, 0.0, 1.0, 0.0]], dtype=float) +# P = np.array([[-530.4669406576809, 0.0, 320.5, -37.13268584603767],[0.0, -530.4669406576809, -240.5, 0.0],[0.0, 0.0, 1.0, 0.0]], dtype=float) +P = np.array([[530.4669406576809, 0.0, 320.5, -37.13268584603767],[0.0, 530.4669406576809, 240.5, 0.0],[0.0, 0.0, 1.0, 0.0]], dtype=float) + +# Os pontos em coordenadas do creeper chegam atraves dos alvar bundles. Só que lá estão em cm e precisam ser convertidos em m +# https://github.com/arnaldojr/my_simulation/blob/master/alvar_bundles/markers_11_azul.xml +tag_big = np.array(([[-0.075,-0.075,0.013,1.0],[0.075,-0.075,0.013,1.0 ],[0.075,0.075,0.013,1.0],[-0.075,0.075,0.013, 1.0]])) +tag_small = np.array([[-0.0149,-0.3423,0.058, 1.0],[0.0149,-0.3423,0.058, 1.0],[0.0149,-0.3111,0.058, 1.0],[-0.0149,-0.3111,0.058, 1.]]) +tags = [tag_big, tag_small] + +def random_color(): + """Retorna uma cor aleatória""" + h = np.random.randint(low=0,high=180) # It's easier to randomize colors in the H component + s = 200 + v = 255 + hsv_img = np.array([[[h,s,v]]], dtype=np.dtype('u1')) # We create a one-pixel image in HSV + #print(hsv_img) + rgb_img = cv2.cvtColor(hsv_img, cv2.COLOR_HSV2RGB) + color_val = rgb_img[0][0].astype(float) + #color_val/=255 + #print(color_val) + return color_val + +def project1_old(M,P,p): + """Converte um ponto de coordenadas camera_rgb_optical_frame para coordenadas de pixel""" + pcamera = np.dot(M, p) + pscreen = np.dot(P, pcamera) + p_float = np.true_divide(pscreen[:-1], pscreen[-1]) + p_int = np.array(p_float, dtype=np.int32) + return p_int + +def project1(M,P,p): + """Converte um ponto de coordenadas camera_rgb_optical_frame para coordenadas de pixel""" + pcamera = np.dot(M, p) + pscreen = np.dot(P, pcamera) + p_float = np.true_divide(pscreen[:-1], pscreen[-1]) + p_int = np.array(p_float, dtype=np.int32) + return p_int + +def convert_and_draw_tags(M,P, tag_points, image, color, id): + """Usando a matriz M que converte do marker para camera_rgb_optical_frame + e a matriz P imutável da câmera + Desenha o contorno de onde o tag foi detectado + """ + points = [] + mx = 0 + my = 0 + for p in tag_points: + # Dá para melhorar este for e este + # project vetorizando - todos os pontos de 1 vez + p_screen = project1(M,P,p) + p_screen[1] = p_screen[1] + ofy + p_screen[0] = p_screen[0] + ofx + mx+=p_screen[0] + my+=p_screen[1] + points.append(list(p_screen)) + sz = len(tag_points) + mx/=sz # daria para melhorar esta conta + my/=sz # se a media fosse feita antes de converter para int + p_array = np.array([points], dtype=np.int32) + cv2.polylines(image, [p_array], True, color,3) + centro_tag = (int(mx), int(my)) + cv2.circle(image, centro_tag,3, color, 3, 8, 0) + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 1 + font_thickness = 2 + cv2.putText(image, str(id), (int(mx)+15, int(my)), font, font_scale, color, font_thickness, cv2.LINE_AA) + + + +def faz_transformacao(ref1, ref2): + """Realiza a transformacao do ponto entre o referencial 1 e o referencial 2 + retorna a trasnformacao + + Para saber todos os referenciais disponíveis veja o frames.pdf gerado por: + + rosrun tf view_frames + """ + possivel_converter = tf_buffer.can_transform(ref1, ref2, rospy.Time(0)) + if possivel_converter: + transf = tf_buffer.lookup_transform(ref1, ref2, rospy.Time(0)) + return transf + else: + return None + +def decompoe(transf): + """Recebe uma transformacao de sistemas de coordenadas e a converte em x,y,z e ângulo em RAD em relação a z""" + # Separa as translacoes das rotacoes + x = transf.transform.translation.x + y = transf.transform.translation.y + z = transf.transform.translation.z + # ATENCAO: tudo o que vem a seguir e' so para calcular um angulo + # Para medirmos o angulo entre marcador e robo vamos projetar o eixo Z do marcador (perpendicular) + # no eixo X do robo (que e' a direcao para a frente) + t = transformations.translation_matrix([x, y, z]) + # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions + r = transformations.quaternion_matrix([transf.transform.rotation.x, transf.transform.rotation.y, transf.transform.rotation.z, transf.transform.rotation.w]) + m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes + z_marker = [0,0,1,0] # Sao 4 coordenadas porque e' um vetor em coordenadas homogeneas + v2 = numpy.dot(m, z_marker) + v2_n = v2[0:-1] # Descartamos a ultima posicao + n2 = v2_n/linalg.norm(v2_n) # Normalizamos o vetor + x_robo = [1,0,0] + cosa = numpy.dot(n2, x_robo) # Projecao do vetor normal ao marcador no x do robo + angulo_marcador_robo = math.acos(cosa) + return x,y,z, angulo_marcador_robo + +def insere_coords_dict(dici, x,y,z,alpha): + " Insere coordenadas (vindas de um transformation) num dicionário para facilitar uso posterior" + dici["x"] = x + dici["y"] = y + dici["z"] = z + dici["alpha"] = alpha + dici["graus"] = math.degrees(alpha) + + +def recebe(msg): + "Recebe o evento das tags alvar" + global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado + global y + global z + global id + + frame_names = {"camera_rgb_optical_frame":"Coordenadas da câmera", "end_effector_link": "Cubo vermelho da mão", "base_link": "Base do robô"} + frame_coords = {"camera_rgb_optical_frame":dict(), "end_effector_link": dict(), "base_link":dict()} + + markers_on_screen = [] + + for marker in msg.markers: + id = marker.id + marcador = "ar_marker_" + str(id) + markers_on_screen.append(id) + + referenciais = frame_names.keys() + for ref in referenciais: + transf = faz_transformacao(ref, marcador) + if transf is not None: + xt, yt, zt, alpha_t = decompoe(transf) + insere_coords_dict(frame_coords[ref], xt, yt, zt, alpha_t) + + for ref in frame_names.keys(): + print("\r") + if ref in frame_coords.keys(): + print("Marcador ", id) + print("No referencial :",ref, " que é ", end=None) + print(frame_names[ref]) + for k in frame_coords[ref].keys(): + print("%s %5.3f"%(k, frame_coords[ref][k]), end=" ") + print() + global marcadores_vistos + marcadores_vistos = markers_on_screen + desenha_tags_tela(marcadores_vistos, cv_image) + + +def desenha_tags_tela(markers_vistos, imagem_bgr): + print("DESENHA ", markers_vistos) + + camera = "camera_rgb_optical_frame" # Se usar a camera da garra troque este nome + for id in markers_vistos: + if id in ids_possiveis_tags: # double check para evitar tags fantasmas + marcador = "ar_marker_" + str(id) + #transf = faz_transformacao(camera, marcador) + transf = faz_transformacao(marcador, camera) + if transf is not None: + # Monta a matriz de traslacao - isso eh trabalhoso no OS + t = transformations.translation_matrix([transf.transform.translation.x, transf.transform.translation.y, transf.transform.translation.z]) + # Encontra as rotacoes e cria uma matriz de rotacao a partir dos quaternions + r = transformations.quaternion_matrix([transf.transform.rotation.x, transf.transform.rotation.y, transf.transform.rotation.z, transf.transform.rotation.w]) + m = numpy.dot(r,t) # Criamos a matriz composta por translacoes e rotacoes que converte do frame do marcador para o frame da camera + for t in tags: + convert_and_draw_tags(m,P, t, imagem_bgr, random_color(), id) + +def projeta(m, coords): + "Recebe uma matriz translacao/rotacao M e lista de coordenadas homogeneas (com 4 elementos), e os projeta na tela" + return numpy.dot(m, coords) + # saida = [] + # for coord in coords: + # proj = numpy.dot(m, coords) + # saida.append(proj) + + + +# A função a seguir é chamada sempre que chega um novo frame +def roda_todo_frame(imagem): + # print("frame") + global cv_image + global media + global centro + global resultados + + try: + antes = time.clock() + temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8") + # Note que os resultados já são guardados automaticamente na variável + # chamada resultados + centro, saida_net, resultados = visao_module.processa(temp_image) + for r in resultados: + # print(r) - print feito para documentar e entender + # o resultado + pass + depois = time.clock() + # Desnecessário - Hough e MobileNet já abrem janelas + cv_image = np.zeros((480*3, 640*3,3 ), dtype=np.uint8) + cv_image[ofy:saida_net.shape[0]+ofy,ofx:saida_net.shape[1]+ofx,:] =saida_net + saida_net.copy() + except CvBridgeError as e: + print('ex', e) + +if __name__=="__main__": + + print(""" + +ALERTA: + + + + + + + +Para funcionar este programa *precisa* do Rviz rodando antes: + +roslaunch turtlebot3_manipulation_moveit_config move_group.launch + +roslaunch my_simulation rviz.launch + +""" + + + ) + + rospy.init_node("cor") + + topico_imagem = "/camera/rgb/image_raw/compressed" + + recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size = 2**24) + recebedor = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, recebe) # Para recebermos notificacoes de que marcadores foram vistos + + + print("Usando ", topico_imagem) + + velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 1) + + tfl = tf2_ros.TransformListener(tf_buffer) #conversao do sistema de coordenadas + tolerancia = 25 + + # Exemplo de categoria de resultados MobileNEt + # [('chair', 86.965459585189819, (90, 141), (177, 265))] + + try: + # Inicializando - por default gira no sentido anti-horário + sentido = 1 + while not rospy.is_shutdown(): + for r in resultados: + pass # Para evitar prints + # print(r) + vel = Twist(Vector3(0.0,0,0), Vector3(0,0,-math.pi/360)) + velocidade_saida.publish(vel) + #sentido*=-1 + + # marcadores_vistos = [] + + if cv_image is not None: + # Note que o imshow precisa ficar *ou* no codigo de tratamento de eventos *ou* no thread principal, não em ambos + # desenha_tags_tela(marcadores_vistos, cv_image) + cv2.imshow("cv_image no loop principal", cv_image) + cv2.waitKey(1) + rospy.sleep(0.05) + + except rospy.ROSInterruptException: + print("Ocorreu uma exceção com o rospy") + + diff --git a/ssd_licia.md b/ssd_licia.md new file mode 100644 index 0000000..57d553c --- /dev/null +++ b/ssd_licia.md @@ -0,0 +1,34 @@ +10:00:54 borg@borg ~ → rospack list | grep -i turtlebot3 +turtlebot3_applications_msgs /opt/ros/melodic/share/turtlebot3_applications_msgs +turtlebot3_automatic_parking /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_automatic_parking +turtlebot3_automatic_parking_vision /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_automatic_parking_vision +turtlebot3_bringup /home/borg/catkin_ws/src/turtlebot3/turtlebot3_bringup +turtlebot3_description /home/borg/catkin_ws/src/turtlebot3/turtlebot3_description +turtlebot3_example /home/borg/catkin_ws/src/turtlebot3/turtlebot3_example +turtlebot3_fake /opt/ros/melodic/share/turtlebot3_fake +turtlebot3_follow_filter /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_follow_filter +turtlebot3_follower /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_follower +turtlebot3_gazebo /opt/ros/melodic/share/turtlebot3_gazebo +turtlebot3_manipulation_bringup /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_bringup +turtlebot3_manipulation_description /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_description +turtlebot3_manipulation_gazebo /home/borg/catkin_ws/src/turtlebot3_manipulation_simulations/turtlebot3_manipulation_gazebo +turtlebot3_manipulation_gui /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_gui +turtlebot3_manipulation_moveit_config /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_moveit_config +turtlebot3_manipulation_navigation /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_navigation +turtlebot3_manipulation_slam /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_slam +turtlebot3_msgs /opt/ros/melodic/share/turtlebot3_msgs +turtlebot3_navigation /home/borg/catkin_ws/src/turtlebot3/turtlebot3_navigation +turtlebot3_panorama /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_panorama +turtlebot3_slam /home/borg/catkin_ws/src/turtlebot3/turtlebot3_slam +turtlebot3_teleop /home/borg/catkin_ws/src/turtlebot3/turtlebot3_teleop +10:01:06 borg@borg ~ → apt list --installed | grep turtlebot3 + +WARNING: apt does not have a stable CLI interface. Use with caution in scripts. + +ros-melodic-turtlebot3-applications-msgs/bionic,now 1.0.0-1bionic.20200320.141228 amd64 [installed] +ros-melodic-turtlebot3-fake/bionic,now 1.2.0-0bionic.20200402.223002 amd64 [installed] +ros-melodic-turtlebot3-fake-dbgsym/bionic,now 1.2.0-0bionic.20200402.223002 amd64 [installed] +ros-melodic-turtlebot3-gazebo/bionic,now 1.2.0-0bionic.20200501.202836 amd64 [installed] +ros-melodic-turtlebot3-gazebo-dbgsym/bionic,now 1.2.0-0bionic.20200501.202836 amd64 [installed] +ros-melodic-turtlebot3-msgs/bionic,now 1.0.0-0bionic.20200304.005554 amd64 [installed] +ros-melodic-turtlebot3-simulations/bionic,now 1.2.0-0bionic.20200501.204018 amd64 [installed] diff --git a/temp/packlist.md b/temp/packlist.md new file mode 100644 index 0000000..fb53b79 --- /dev/null +++ b/temp/packlist.md @@ -0,0 +1,127 @@ +```bash +04:15:44 borg@borg revisao2020 ±|master ✗|→ rospack list | grep -i turtlebot3 +open_manipulator_with_tb3_description /home/borg/catkin_ws/src/turtlebot3_manipulation_tb3/open_manipulator_with_tb3_description +open_manipulator_with_tb3_msgs /home/borg/catkin_ws/src/turtlebot3_manipulation_tb3/open_manipulator_with_tb3_msgs +open_manipulator_with_tb3_tools /home/borg/catkin_ws/src/turtlebot3_manipulation_tb3/open_manipulator_with_tb3_tools +open_manipulator_with_tb3_waffle_moveit /home/borg/catkin_ws/src/turtlebot3_manipulation_tb3/open_manipulator_with_tb3_waffle_moveit +open_manipulator_with_tb3_waffle_pi_moveit /home/borg/catkin_ws/src/turtlebot3_manipulation_tb3/open_manipulator_with_tb3_waffle_pi_moveit +turtlebot3_applications_msgs /opt/ros/melodic/share/turtlebot3_applications_msgs +turtlebot3_automatic_parking /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_automatic_parking +turtlebot3_automatic_parking_vision /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_automatic_parking_vision +turtlebot3_autorace_camera /opt/ros/melodic/share/turtlebot3_autorace_camera +turtlebot3_autorace_control /opt/ros/melodic/share/turtlebot3_autorace_control +turtlebot3_autorace_core /opt/ros/melodic/share/turtlebot3_autorace_core +turtlebot3_autorace_detect /opt/ros/melodic/share/turtlebot3_autorace_detect +turtlebot3_bringup /home/borg/catkin_ws/src/turtlebot3/turtlebot3_bringup +turtlebot3_description /home/borg/catkin_ws/src/turtlebot3/turtlebot3_description +turtlebot3_example /home/borg/catkin_ws/src/turtlebot3/turtlebot3_example +turtlebot3_fake /opt/ros/melodic/share/turtlebot3_fake +turtlebot3_follow_filter /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_follow_filter +turtlebot3_follower /home/borg/catkin_ws/src/turtlebot3_applications/turtlebot3_follower +turtlebot3_gazebo /opt/ros/melodic/share/turtlebot3_gazebo +turtlebot3_manipulation_bringup /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_bringup +turtlebot3_manipulation_description /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_description +turtlebot3_manipulation_gazebo /home/borg/catkin_ws/src/turtlebot3_manipulation_simulations/turtlebot3_manipulation_gazebo +turtlebot3_manipulation_gui /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_gui +turtlebot3_manipulation_moveit_config /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_moveit_config +turtlebot3_manipulation_navigation /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_navigation +turtlebot3_manipulation_slam /home/borg/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_slam +turtlebot3_msgs /opt/ros/melodic/share/turtlebot3_msgs +turtlebot3_navigation /home/borg/catkin_ws/src/turtlebot3/turtlebot3_navigation +turtlebot3_panorama /opt/ros/melodic/share/turtlebot3_panorama +turtlebot3_slam /home/borg/catkin_ws/src/turtlebot3/turtlebot3_slam +turtlebot3_teleop /home/borg/catkin_ws/src/turtlebot3/turtlebot3_teleop + +04:15:55 borg@borg revisao2020 ±|master ✗|→ apt list --installed | grep turtlebot3 + +WARNING: apt does not have a stable CLI interface. Use with caution in scripts. + +ros-melodic-turtlebot3/bionic,now 1.2.2-1bionic.20200406.134636 amd64 [installed] +ros-melodic-turtlebot3-applications-msgs/bionic,now 1.0.0-1bionic.20200320.141228 amd64 [installed] +ros-melodic-turtlebot3-automatic-parking/bionic,now 1.1.0-0bionic.20200320.132517 amd64 [installed] +ros-melodic-turtlebot3-autorace/bionic,now 1.2.0-0bionic.20200320.153552 amd64 [installed] +ros-melodic-turtlebot3-autorace-camera/bionic,now 1.2.0-0bionic.20200320.135005 amd64 [installed] +ros-melodic-turtlebot3-autorace-control/bionic,now 1.2.0-0bionic.20200320.151434 amd64 [installed] +ros-melodic-turtlebot3-autorace-core/bionic,now 1.2.0-0bionic.20200320.111412 amd64 [installed] +ros-melodic-turtlebot3-autorace-detect/bionic,now 1.2.0-0bionic.20200320.150858 amd64 [installed] +ros-melodic-turtlebot3-bringup/bionic,now 1.2.2-1bionic.20200402.222853 amd64 [installed] +ros-melodic-turtlebot3-bringup-dbgsym/bionic,now 1.2.2-1bionic.20200402.222853 amd64 [installed] +ros-melodic-turtlebot3-description/bionic,now 1.2.2-1bionic.20200320.113528 amd64 [installed] +ros-melodic-turtlebot3-example/bionic,now 1.2.2-1bionic.20200406.134126 amd64 [installed] +ros-melodic-turtlebot3-fake/bionic,now 1.2.0-0bionic.20200402.223002 amd64 [installed] +ros-melodic-turtlebot3-fake-dbgsym/bionic,now 1.2.0-0bionic.20200402.223002 amd64 [installed] +ros-melodic-turtlebot3-follow-filter/bionic,now 1.1.0-0bionic.20200408.172513 amd64 [installed] +ros-melodic-turtlebot3-follower/bionic,now 1.1.0-0bionic.20200320.132529 amd64 [installed] +ros-melodic-turtlebot3-gazebo/now 1.2.0-0bionic.20200320.144257 amd64 [installed,upgradable to: 1.2.0-0bionic.20200501.202836] +ros-melodic-turtlebot3-gazebo-dbgsym/now 1.2.0-0bionic.20200320.144257 amd64 [installed,upgradable to: 1.2.0-0bionic.20200501.202836] +ros-melodic-turtlebot3-msgs/bionic,now 1.0.0-0bionic.20200304.005554 amd64 [installed] +ros-melodic-turtlebot3-navigation/bionic,now 1.2.2-1bionic.20200402.223304 amd64 [installed] +ros-melodic-turtlebot3-panorama/bionic,now 1.1.0-0bionic.20200402.223439 amd64 [installed] +ros-melodic-turtlebot3-panorama-dbgsym/bionic,now 1.1.0-0bionic.20200402.223439 amd64 [installed] +ros-melodic-turtlebot3-simulations/now 1.2.0-0bionic.20200402.223654 amd64 [installed,upgradable to: 1.2.0-0bionic.20200501.204018] +ros-melodic-turtlebot3-slam/bionic,now 1.2.2-1bionic.20200402.223305 amd64 [installed] +ros-melodic-turtlebot3-slam-dbgsym/bionic,now 1.2.2-1bionic.20200402.223305 amd64 [installed] +ros-melodic-turtlebot3-teleop/bionic,now 1.2.2-1bionic.20200320.105738 amd64 [installed] + +04:19:33 borg@borg revisao2020 ±|master ✗|→ apt list -a ros-melodic-turtlebot3* +Listing... Done +ros-melodic-turtlebot3/bionic,now 1.2.2-1bionic.20200406.134636 amd64 [installed] + +ros-melodic-turtlebot3-applications/bionic 1.1.0-0bionic.20200514.235302 amd64 + +ros-melodic-turtlebot3-applications-msgs/bionic,now 1.0.0-1bionic.20200320.141228 amd64 [installed] + +ros-melodic-turtlebot3-automatic-parking/bionic,now 1.1.0-0bionic.20200320.132517 amd64 [installed] + +ros-melodic-turtlebot3-automatic-parking-vision/bionic 1.1.0-0bionic.20200514.233738 amd64 + +ros-melodic-turtlebot3-autorace/bionic,now 1.2.0-0bionic.20200320.153552 amd64 [installed] + +ros-melodic-turtlebot3-autorace-camera/bionic,now 1.2.0-0bionic.20200320.135005 amd64 [installed] + +ros-melodic-turtlebot3-autorace-control/bionic,now 1.2.0-0bionic.20200320.151434 amd64 [installed] + +ros-melodic-turtlebot3-autorace-core/bionic,now 1.2.0-0bionic.20200320.111412 amd64 [installed] + +ros-melodic-turtlebot3-autorace-detect/bionic,now 1.2.0-0bionic.20200320.150858 amd64 [installed] + +ros-melodic-turtlebot3-bringup/bionic,now 1.2.2-1bionic.20200402.222853 amd64 [installed] + +ros-melodic-turtlebot3-bringup-dbgsym/bionic,now 1.2.2-1bionic.20200402.222853 amd64 [installed] + +ros-melodic-turtlebot3-description/bionic,now 1.2.2-1bionic.20200320.113528 amd64 [installed] + +ros-melodic-turtlebot3-example/bionic,now 1.2.2-1bionic.20200406.134126 amd64 [installed] + +ros-melodic-turtlebot3-fake/bionic,now 1.2.0-0bionic.20200402.223002 amd64 [installed] + +ros-melodic-turtlebot3-fake-dbgsym/bionic,now 1.2.0-0bionic.20200402.223002 amd64 [installed] + +ros-melodic-turtlebot3-follow-filter/bionic,now 1.1.0-0bionic.20200408.172513 amd64 [installed] + +ros-melodic-turtlebot3-follower/bionic,now 1.1.0-0bionic.20200320.132529 amd64 [installed] + +ros-melodic-turtlebot3-gazebo/bionic 1.2.0-0bionic.20200501.202836 amd64 [upgradable from: 1.2.0-0bionic.20200320.144257] +ros-melodic-turtlebot3-gazebo/now 1.2.0-0bionic.20200320.144257 amd64 [installed,upgradable to: 1.2.0-0bionic.20200501.202836] + +ros-melodic-turtlebot3-gazebo-dbgsym/bionic 1.2.0-0bionic.20200501.202836 amd64 [upgradable from: 1.2.0-0bionic.20200320.144257] +ros-melodic-turtlebot3-gazebo-dbgsym/now 1.2.0-0bionic.20200320.144257 amd64 [installed,upgradable to: 1.2.0-0bionic.20200501.202836] + +ros-melodic-turtlebot3-msgs/bionic,now 1.0.0-0bionic.20200304.005554 amd64 [installed] + +ros-melodic-turtlebot3-navigation/bionic,now 1.2.2-1bionic.20200402.223304 amd64 [installed] + +ros-melodic-turtlebot3-panorama/bionic,now 1.1.0-0bionic.20200402.223439 amd64 [installed] + +ros-melodic-turtlebot3-panorama-dbgsym/bionic,now 1.1.0-0bionic.20200402.223439 amd64 [installed] + +ros-melodic-turtlebot3-simulations/bionic 1.2.0-0bionic.20200501.204018 amd64 [upgradable from: 1.2.0-0bionic.20200402.223654] +ros-melodic-turtlebot3-simulations/now 1.2.0-0bionic.20200402.223654 amd64 [installed,upgradable to: 1.2.0-0bionic.20200501.204018] + +ros-melodic-turtlebot3-slam/bionic,now 1.2.2-1bionic.20200402.223305 amd64 [installed] + +ros-melodic-turtlebot3-slam-dbgsym/bionic,now 1.2.2-1bionic.20200402.223305 amd64 [installed] + +ros-melodic-turtlebot3-teleop/bionic,now 1.2.2-1bionic.20200320.105738 amd64 [installed] +``` +