| Ubicación | Bloque 19, 19-213 |
|---|---|
| Fecha | 2022-10-18 |
| Hora | 10:00 a.m |
| Asistentes | Davinson Castaño, Joan Alvarado, Joan Beltran, Maria Camila Maldonado, Stefania Valencia |
A continuación se detalla la implementación de breakout a día de hoy
import rclpy
import numpy
from math import radians
from random import random
from rclpy.node import Node
from turtlesim.srv import Kill, Spawn, TeleportAbsolute, SetPen
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])
self.block_number = self.blocks.sum()
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 SETPEN
self.pen = {
'ball': self.create_client(SetPen, 'ball/set_pen'),
'bar' : self.create_client(SetPen, 'bar/set_pen')
}
self.req = SetPen.Request()
self.set_pen(off=1,obj='ball')
self.set_pen(off=1,obj='bar')
# 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))
self.block_number = self.blocks.sum()
self.get_logger().info('Tortugas faltantes : %d'%self.block_number)
############### 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)
if y < 0.5 and self.hit != 'bottom':
self.hit = 'bottom'
self.lifes -= 1
self.get_logger().info('Vidas faltantes : %d'%self.lifes)
self.started = False
self.ball_vx = 0.0
self.teleport(self.bar_x,1.5,radians(60+60*random()))
self.hit = 'bar'
self.bar_pub.publish(Twist())
newmsg = Twist()
if self.ball_vx > 0:
self.ball_vx = max(2.0,y)
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(self.bar_x,1.5,radians(60+60*random()))
self.hit = 'bar'
self.bar_pub.publish(newmsg)
def set_pen(self, r=255, g=255, b=255, w=1, off=0, wait=False, obj='ball'):
self.req.r = r
self.req.g = g
self.req.b = b
self.req.width = w
self.req.off = off
self.future = self.pen[obj].call_async(self.req)
if wait:
rclpy.spin_until_future_complete(self, self.future)
return self.future.result()
####################################################################
# 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()
A continuación se presenta la implementación de archivo Launch para correr el programa el cual esta gurdado dentro del paquete en la carpeta launch:
from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
pkg_dir = get_package_share_directory('rebote')
def generate_launch_description():
turtlesim_node = Node(
package='turtlesim',
executable='turtlesim_node',
name='window',
output='own_log',
parameters=[
{'background_r':0},
{'background_g':0},
{'background_b':0},
]
)
breakout_node = Node(
package='rebote',
executable='breakout',
name='breakout',
)
# teleop_node = Node(
# package='turtlesim',
# executable='turtle_teleop_key',
# name='teleop',
# )
return LaunchDescription([
turtlesim_node,
breakout_node,
# teleop_node
])
Se modifica el archivo setup.py del paquete con lo siguiente:
# Al inicio del archivo se importa la libreria glob
from glob import glob
# Data files en la función setup
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name, glob('launch/*.launch.py')),
('share/' + package_name, glob('config/*.rviz')),
],
Despues de la modificaciones corremos el programa en el terminal de la siguiente forma:
# En terminal 1
cd ~/ares_ws
colcon build && source install/setup.bash
ros2 launch rebote breakout.launch.py
# En terminal 2
ros2 run turtlesim turtle_teleop_key