Semana 63. Fase 3 del ejercicio Amazon Warehouse

Fase 3 del ejercicio Amazon Warehouse

Por un lado, he incorporado los mundos que utilizan el robot Ackermann en el selector de mundos del ejercicio. Ha sido necesario actualizar las funciones getRobotCoordinates() y getRobotAngle() en la plantilla map.py, debido a que la escala en los dos mundos varía significativamente, y la desviación del error en el ángulo de los robots difiere entre uno y otro:

def getRobotCoordinates(self):
  pose = self.pose3d.getPose3d()

  x = pose.x
  y = pose.y

  if self.circuit == "World2" or self.circuit == "World4":  # Warehouse 2
    '''
    x = (h_gazebo / 2 - x) * (h_image / h_gazebo) * (h_canvas / h_image)
    y = (w_gazebo / 2 - y) * (w_image / w_gazebo) * (w_canvas / w_image)
    '''
    x = (12.0 - x) * 29.13 * 0.22
    y = (17.0 - y) * 31.62 * 0.28

  else:	# Warehouse 1 (small one)
    x = (6.8 - x) * 20.22 * 0.545
    y = (10.31 - y) * 20.17 * 0.72

  return y, x

def getRobotAngle(self):
  pose = self.pose3d.getPose3d()

  if self.circuit == "World3" or self.circuit == "World4":  # Warehouses with Ackermann robot 
    rt = pose.yaw - 1.74
  else: # Warehouses with holonomic robot
    rt = pose.yaw - 1.24
  ty = math.cos(-rt) - math.sin(-rt)
  tx = math.sin(-rt) + math.cos(-rt)

  return tx, ty

Esta modificación ha implicado la necesidad de pasar el parámetro circuit. Para ello, ha sido necesario modificar entrypoint/ros2_humble/exercise.py con el fin de proporcionar el parámetro circuit a los scripts exercise.py y gui.py.

exercise_command=f"{current_path}/../../python_template/ros2_humble/exercise.py 0.0.0.0 {circuit}"

Por otro lado, he avanzado en el desarollo de la solución de referencia para la fase 3. Como punto de partida, opté por el espacio de estado Reeds-Shepp, que, si bien es similar al espacio de estado Dubins, amplía el concepto incluyendo el movimiento de marcha atrás. A pesar de que conlleve a soluciones de rutas más complejas, resulta ser mucho más eficiente para un robot Ackermann, dado que este tiene restricciones de movimientos más estrictas y se puede aprovechar al máximo las capacidades de maniobra del robot. A continuación, muestro uno de los resultados obtenido:

El siguiente paso implica programar el pilotaje del robot para que este pueda completar la ruta calculada previamente.

Memoria del TFG

Por último, ya empezamos a redactar la memoria del proyecto, siendo la primera tarea la elaboración del índice de la misma.