# Exemplos básicos - Docker IC

## :ghost:

OS: Ubuntu 20.04
ROS: Noetic (ROS 1)

Autor: Lucca Gandra ([e-mail])

## Recebimento de imagem e processamento:

Neste código, há o recebimento de uma imagem de profundidade e o acesso ao valor de distância até o pixel. No caso, Xc e Yc são o centro da imagem, mas poderia ser qualquer valor dentro do tamanho da figura.

Cx, Cy, Fx, Fy são parâmetros de distorção da imagem.


#!/usr/bin/env python 

# Imports
import rospy
from sensor_msgs.msg import Image
from nav_msgs.msg import Odometry
from cv_bridge import CvBridge, CvBridgeError
import cv2

class EstimateDistance:
    def __init__(self):
        rospy.init_node('estimate_distance')

        self.bridge = CvBridge()

        self.pub_image_mod = rospy.Publisher('/icts_uav/camera_down/depth/image_distance', Image, queue_size=10)
        
        rospy.Subscriber("/mavros/global_position/local", Odometry, self.update_z)
        rospy.Subscriber("/icts_uav/camera_down/depth/image_raw", Image, self.callback)
        
        self.z = 0

        rospy.spin()

    def update_z(self, msg):
        self.z = msg.pose.pose.position.z

    def callback(self, msg):
        try:
            image = self.bridge.imgmsg_to_cv2(msg, "32FC1")
        except CvBridgeError as e:
            print(e)

        height, width = image.shape[:2]

        #Xc = ((xmax-xmin)/2)+xmin
        #Yc = ((ymax-ymin)/2)+ymin

        Xc = round(width/2)  # 320
        Yc = round(height/2) # 240
        #Xc = 200
        #Yc = 20

        Cx = 320.5
        Cy = 240.5
        fx = 381.36246688113556
        fy = 381.36246688113556

        Z = image[Xc,Yc]

        Xreal=((Xc-Cx)*Z)/fx
        Yreal=((Yc-Cy)*Z)/fy
        
        self.pub_image_mod.publish(self.bridge.cv2_to_imgmsg(image, "32FC1"))

if __name__ == '__main__':
    obj = EstimateDistance()

## Envio de um setpoint de posição:

O input do usuário é passado como setpoint de posição.

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler

class SendGoalObstacle:
    def __init__(self):

        rospy.init_node('goal_send_planners')

        self.send_goal_mavros = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=1, latch=True)

        self.quat_desired = quaternion_from_euler (0, 0, 3.14159)

            self.x, self.y, self.z, self.yaw = input("x y z yaw(rad): ").split()
            self.callback()

        self.x_current = 0
        self.y_current = 0
        self.z_current = 0

        rospy.spin()


    def callback(self):

        send_goal = PoseStamped()


        send_goal.pose.position.x = float(self.y)
        send_goal.pose.position.y = float(self.x)
        send_goal.pose.position.z = float(self.z)   

        quaternion = quaternion_from_euler(0,0,float(self.yaw)) # Roll, Pitch e Yaw

        send_goal.pose.orientation.x = quaternion[0]
        send_goal.pose.orientation.y = quaternion[1]
        send_goal.pose.orientation.z = quaternion[2]
        send_goal.pose.orientation.w = quaternion[3]

        #send_goal.header.seq = "1"
        #send_goal.header.stamp = ""
        send_goal.header.frame_id = 'world' 

        self.send_goal_mavros.publish(send_goal)

if __name__ == '__main__':
    loop = SendGoalObstacle()