Acta de reunión

# Inicialización
cd ~/ares_ws/
colcon build && source install/setup.bash 

# Corremos los tres siguiente programas en tres terminales diferentes
ros2 run rebote breakout 
ros2 run turtlesim turtlesim_node --ros-args -p background_r:=0 -p background_g:=0  -p background_b:=0
ros2 run turtlesim turtle_teleop_key 

import rclpy
import numpy

from math import radians
from random import random

from rclpy.node import Node
from turtlesim.srv import Kill, Spawn, TeleportAbsolute
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

class Nodo(Node):

    def __init__(self):
        super().__init__('breakout')
        self.get_logger().info('Breakout game ready')

        # SERVICE CLIENT TO KILL    

        self.kill_cli = self.create_client(Kill, 'kill')
        while not self.kill_cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Servicio no disponible, esperando ...')
        self.kill_req = Kill.Request()      

        self.doKill('turtle1',wait=True)

        # SERVICE CLIENT TO SPAWN    

        self.spawn_cli = self.create_client(Spawn, 'spawn')
        while not self.spawn_cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Servicio no disponible, esperando ...')
        self.spawn_req = Spawn.Request()      

        self.blocks = numpy.ones([4,11])

        for f,fila in enumerate(self.blocks):
            for c,block in enumerate(fila):
                self.doSpawn('bloque_%d_%d'%(f,c),x=c*1.0+0.5,y=10.5-f*1.0,theta=radians(270),wait=True)

        self.doSpawn('bar',x=5.5,y=0.5,theta=radians(90),wait=True) 
        self.doSpawn('ball',x=5.5,y=1.5,theta=radians(90),wait=True)    

        # SERVICE CLIENT TO TELEPORT

        self.teleport_cli = self.create_client(TeleportAbsolute, '/ball/teleport_absolute')
        while not self.teleport_cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Servicio no disponible, esperando ...')
        self.teleport_req = TeleportAbsolute.Request()

        # TOPIC PUBLISHERS

        self.ball_pub = self.create_publisher(Twist, 'ball/cmd_vel', 1)
        self.bar_pub  = self.create_publisher(Twist, 'bar/cmd_vel', 1)

        # TOPIC SUBSCRIBERS

        self.ball_sub = self.create_subscription(Pose,'ball/pose',self.ball_callback,10)
        self.ball_sub  # prevent unused variable warning
        self.bar_sub = self.create_subscription(Pose,'bar/pose',self.bar_callback,10)
        self.bar_sub  # prevent unused variable warning
        self.vel_sub = self.create_subscription(Twist,'turtle1/cmd_vel',self.vel_callback,10)
        self.vel_sub  # prevent unused variable warning

        # PARAMETERS

        self.bar_x  = 5.5
        self.lifes  = 3

        self.ball_vx = 0.0

        self.started = False
        self.hit = False

    def doKill(self, name, wait=False):
        self.kill_req.name = name
        self.future = self.kill_cli.call_async(self.kill_req)
        if wait:
            rclpy.spin_until_future_complete(self, self.future)
            return self.future.result()

    def doSpawn(self, name, x=5.5, y=5.5, theta=0.0, wait=False):
        self.spawn_req.name = name
        self.spawn_req.x = x
        self.spawn_req.y = y
        self.spawn_req.theta = theta
        self.future = self.spawn_cli.call_async(self.spawn_req)
        if wait:
            rclpy.spin_until_future_complete(self, self.future)
            return self.future.result()

    def teleport(self, x, y, theta, wait=False):
        self.teleport_req.x = x
        self.teleport_req.y = y
        self.teleport_req.theta = theta
        self.future = self.teleport_cli.call_async(self.teleport_req)
        if wait:
            rclpy.spin_until_future_complete(self, self.future)
            return self.future.result()

    def bar_callback(self, msg):
        self.bar_x = msg.x
        # self.get_logger().info('Barra en x=%f'%self.bar_x)

    def ball_callback(self, msg):
        x = msg.x
        y = msg.y
        theta = msg.theta

        for f,fila in enumerate(self.blocks):
            for c,block in enumerate(fila):
                xb = c*1.0+0.5
                yb = 10.5-f*1.0

                dx = (x-xb)
                dy = (y-yb)

                alpha = numpy.arctan2(dy,dx)

                r2 = dx**2 + dy**2

                if r2 < 0.5**2 and self.blocks[f][c]==1 :
                    self.hit = 'block'
                    self.doKill('bloque_%d_%d'%(f,c))
                    self.blocks[f][c] = 0
                    self.teleport(x,y,2*alpha-theta+radians(180))

        ############### BARRA
        xb = self.bar_x
        yb = 0.5

        dx = (x-xb)
        dy = (y-yb)

        alpha = numpy.arctan2(dy,dx)

        r2 = dx**2 + dy**2

        if r2 < 2 and self.hit !='bar':
            self.hit = 'bar'
            self.teleport(x,y,2*alpha-theta+radians(180))
        ################# BARRA END

        if x > 10.5 and self.hit !='right':
            self.hit = 'right'
            alpha = radians(180)
            rebote = (2*alpha-theta+radians(180))
            self.teleport(x,y,rebote)
        if x < 0.5 and self.hit !='left':
            self.hit = 'left'
            alpha = 0
            rebote = 2*alpha-theta+radians(180)
            self.teleport(x,y,rebote)
        if y > 10.5 and self.hit !='top':
            self.hit = 'top'
            alpha = radians(270)
            rebote = 2*alpha-theta+radians(180)
            self.teleport(x,y,rebote)

        newmsg = Twist()
        newmsg.linear.x = self.ball_vx
        self.ball_pub.publish(newmsg)

    def vel_callback(self, msg):
        newmsg = Twist()
        newmsg.linear.y = msg.angular.z
        if not self.started:
            self.started = True
            self.ball_vx = 2.0
            self.teleport(5.5,1.5,radians(60+60*random()))
        self.bar_pub.publish(newmsg)


####################################################################
# No es necesario modificar lo que se presenta a continuación:
####################################################################

def main(args=None):            # Función main
    rclpy.init(args=args)        # Iniciamos la comunicacion con ROS2
    node = Nodo()                # Creamos el objeto nodo
    rclpy.spin(node)            # Ejectutamos el nodo en un loop
    node.destroy_node()            # Cuando salimos del nodo lo destruimos
    rclpy.shutdown()            # Cerrramos la comunicacion con ROS2

if __name__ == '__main__':        # Si ejecutamos el archivo de python
    main()                        # Ejecutamos la funcion main()