def main(args=None):
#BeginSquare with the current position of robot 0
s = pose_to_square(World().allies[0].pose.position.x, World().allies[0].pose.position.y)
SquareMap[s[0]][s[1]]=1
#Square Arrival
SquareMap[32][21]=2
#All obstacles
for i in range(len(SquareMap)):
for j in range(len(SquareMap[i])):
RobotisInSquare(SquareMap[i][j])
# Create the path to SquareArrival
path = a_star(SquareMap)
#Transform path with squares in path with poses
for i in range(len(path)) :
path[i]= square_to_pose(path[i])
# There is a moveTo to every poses of the path
# The robot will go to every poses(squares) one by one (without interuption)
moveTo(path)
class Control(Strategies):
def __init__(self, id: float, x: float, y: float, theta: float) -> None:
self.x = x
self.id = id
self.y = y
self.theta = theta
self.publisher_ = World().node_.create_publisher(
Commands, 'commands', 1)
def command(self, x, y):
commands = Commands()
command = Command()
command.id = self.id
hardware = Hardware()
hardware.kick_power = 0.0
hardware.kick_type = Hardware.NO_KICK
command.hardware = hardware
command.velocity.linear.x = x
command.velocity.linear.y = y
command.velocity.angular.z = 0.0
commands.commands.append(command)
self.publisher_.publish(commands)
def update(self):
robot: Robot = World().allies[self.id]
rob_target_x = robot.pose.position.x - self.x
rob_target_y = robot.pose.position.y - self.y
self.command(rob_target_x, rob_target_y)
def __init__(self, id: float, x: float, y: float, theta: float) -> None:
self.x = x
self.id = id
self.y = y
self.theta = theta
self.publisher_ = World().node_.create_publisher(
Commands, 'commands', 1)
def state_change(self) -> bool:
'''
See if the state is changed since the last update.
'''
if World().gc.command is None or self.last_gc_receive == World(
).gc.command:
return False
else:
self.last_gc_receive = World().gc.command
return True
def RobotisInSquare(square):
radius=0.085
distRobot= 6+radius
pose= square_to_pose(square)
for i in range(6):
x = World().allies[i].pose.position.x
y = World().allies[i].pose.position.y
xO = World().allies[i].pose.position.x
yO = World().allies[i].pose.position.y
if distance(x,y,pose[0],pose[1])<distRobot or distance(xO,yO,pose[0],pose[1])<distRobot :
if i!=0:
SquareMap[square[0]][square[1]]= -1 #-1 for obstacles
elif distance(x,y,pose[0],pose[1])>distRobot: #if robot not yellow and id=0
SquareMap[square[0]][square[1]]= -1
def CreateObstacleRobots(node):
radius=0.085
distRobot= 6+radius
for i in range(98) :
for j in range(68) :
pose = node_to_pose(node)
for r in range(6):
x = World().allies[r].pose.position.x
y = World().allies[r].pose.position.y
xO = World().allies[r].pose.position.x
yO = World().allies[r].pose.position.y
if distance(x,y,pose[0],pose[1])<distRobot or distance(xO,yO,pose[0],pose[1])<distRobot :
if i!=0:
obstacles.append((i,j))
elif distance(x,y,pose[0],pose[1])>distRobot: #if robot not yellow and id=0
obstacles.append((i,j))
def update(self):
if not (World().ready()):
self.get_logger().info('[WAITING] : All main topic not receive')
return
if self.state_change():
self.callback_stop()
self.update_state()
self.callback_state()
def update(self):
robot: Robot = World().allies[self.id]
rob_target_x = robot.pose.position.x - self.x
rob_target_y = robot.pose.position.y - self.y
self.command(rob_target_x, rob_target_y)
def update_state(self):
if World().gc.command == Command.HALT.value:
self.get_logger().info('State changed : HALT')
self.internal_state_ = State.HALT
self.start_halt()
elif World().gc.command == Command.STOP.value:
self.get_logger().info('State changed : STOP')
self.internal_state_ = State.STOP
self.start_stop()
elif World().gc.command == Command.PREPARE_KICKOFF_BLUE.value:
self.get_logger().info('State changed : PREPARE_KICKOFF_BLUE')
if self.is_yellow:
self.internal_state_ = State.KICKOFF_OPPONENT
self.start_kickoff_opponent()
else:
self.internal_state_ == State.KICKOFF_ALLY
self.start_kickoff_ally()
elif World().gc.command == Command.PREPARE_KICKOFF_YELLOW.value:
self.get_logger().info('State changed : PREPARE_KICKOFF_YELLOW')
if self.is_yellow:
self.internal_state_ = State.KICKOFF_ALLY
self.start_kickoff_ally()
else:
self.internal_state_ == State.KICKOFF_OPPONENT
self.start_kickoff_opponent()
elif World().gc.command == Command.NORMAL_START.value:
self.get_logger().info('State changed : NORMAL_START')
self.internal_state_ = State.NORMAL_START
self.start_normal_start()
elif World().gc.command == Command.FORCE_START.value:
self.get_logger().info('State changed : FORCE_START')
self.internal_state_ = State.NORMAL_START
self.start_force_start()
elif World().gc.command == Command.PREPARE_PENALTY_BLUE.value:
self.get_logger().info('State changed : PREPARE_PENALTY_BLUE')
if self.is_yellow:
self.internal_state_ = State.PENALTY_OPPONENT
self.start_penalty_opponent()
else:
self.internal_state_ = State.PENALTY_ALLY
self.start_penalty_ally()
elif World().gc.command == Command.PREPARE_PENALTY_YELLOW.value:
self.get_logger().info('State changed : PREPARE_PENALTY_YELLOW')
if self.is_yellow:
self.internal_state_ = State.PENALTY_ALLY
self.start_penalty_ally()
else:
self.internal_state_ = State.PENALTY_OPPONENT
self.start_penalty_opponent()
elif World().gc.command == Command.DIRECT_FREE_BLUE.value:
self.get_logger().info('State changed : DIRECT_FREE_BLUE')
if self.is_yellow:
self.internal_state_ = State.DIRECT_FREE_OPPONENT
self.start_direct_free_opponent()
else:
self.internal_state_ = State.DIRECT_FREE_ALLY
self.start_direct_free_ally()
elif World().gc.command == Command.DIRECT_FREE_YELLOW.value:
self.get_logger().info('State changed : DIRECT_FREE_YELLOW')
if self.is_yellow:
self.internal_state_ = State.DIRECT_FREE_ALLY
self.start_direct_free_ally()
else:
self.internal_state_ = State.DIRECT_FREE_OPPONENT
self.start_direct_free_opponent()
elif World().gc.command == Command.INDIRECT_FREE_BLUE.value:
self.get_logger().info('State changed : INDIRECT_FREE_BLUE')
if self.is_yellow:
self.internal_state_ = State.INDIRECT_FREE_OPPONENT
self.start_indirect_free_opponent()
else:
self.internal_state_ = State.INDIRECT_FREE_ALLY
self.start_indirect_free_ally()
elif World().gc.command == Command.INDIRECT_FREE_YELLOW.value:
self.get_logger().info('State changed : INDIRECT_FREE_YELLOW')
if self.is_yellow:
self.internal_state_ = State.INDIRECT_FREE_ALLY
self.start_indirect_free_ally()
else:
self.internal_state_ = State.INDIRECT_FREE_OPPONENT
self.start_indirect_free_opponent()
elif World().gc.command == Command.TIMEOUT_BLUE.value or World(
).gc.command == Command.TIMEOUT_YELLOW.value:
self.internal_state_ = State.TIMEOUT
self.start_timeout()
else:
self.get_logger().warn('[START] Command not implemented : %d' %
World().gc.command)
def __init__(self, id: float, x: float, y: float, theta: float) -> None:
self.client_ = ActionClient(World().node_, Behavior,
f"robot_{id}/behavior")
self.client_.wait_for_server()
self.client_.send_goal_async(create_move_to(x, y, theta))