def push_ball(self):
# self.debug.add_log(1, "Grab ball called")
# self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm))
if (self.last_ball_position - self.player.pose.position).norm < 40:
self.next_state = self.halt
self.last_time = time.time()
elif self._is_player_opposing_ball_and_target(-0.9):
self.next_state = self.push_ball
else:
self.next_state = self.get_behind_ball
# self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle))
target = self.target.position
player = self.player.pose.position
player_to_target = target - player
player_to_target = 0.5 * player_to_target / np.linalg.norm(player_to_target)
speed_pose = Pose(Position.from_array(player_to_target))
return MoveTo(speed_pose)
def push_ball(self):
# self.debug.add_log(1, "Grab ball called")
# self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm))
if (self.last_ball_position - self.player.pose.position).norm < 40:
self.next_state = self.halt
self.last_time = time.time()
elif self._is_player_opposing_ball_and_target(-0.9):
self.next_state = self.push_ball
else:
self.next_state = self.get_behind_ball
# self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle))
target = self.target.position
player = self.player.pose.position
player_to_target = target - player
player_to_target = 0.5 * player_to_target / np.linalg.norm(
player_to_target)
speed_pose = Pose(Position.from_array(player_to_target))
return MoveTo(speed_pose)
def test_givenNumpyArray_whenFromArray_thenPositionIsCopy(self):
pos = Position.from_array(A_ARRAY)
self.assertIsNot(pos, A_ARRAY)
def test_givenNumpyArray_whenFromArray_thenReturnNewPosition(self):
pos = Position.from_array(A_ARRAY)
self.assertEqual(pos.x, A_ARRAY[0])
self.assertEqual(pos.y, A_ARRAY[1])
def from_array(cls, start: ndarray, target: ndarray) -> 'Path':
if start.size != 2 or start.size != 2:
raise ValueError('Cannot create a path with less then two points')
return cls(Position.from_array(start), Position.from_array(target))
def test_givenNumpyArray_whenFromArray_thenPositionIsCopy(self):
pos = Position.from_array(A_ARRAY)
self.assertIsNot(pos, A_ARRAY)
def test_givenNumpyArray_whenFromArray_thenReturnNewPosition(self):
pos = Position.from_array(A_ARRAY)
self.assertEqual(pos.x, A_ARRAY[0])
self.assertEqual(pos.y, A_ARRAY[1])