def _replace_robot_angle(self, mouse_pos):
# ロボット角度のReplacement
field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())
# ロボット角度をradiansからdegreesに変換する
robot_point = QPointF()
if self._replace_is_yellow:
robot_point = QPointF(
self._robot_info['yellow'][self._replace_id].pose.x,
self._robot_info['yellow'][self._replace_id].pose.y)
else:
robot_point = QPointF(
self._robot_info['blue'][self._replace_id].pose.x,
self._robot_info['blue'][self._replace_id].pose.y)
robot = ReplaceRobot()
robot.x = robot_point.x()
robot.y = robot_point.y()
robot.dir = math.degrees(self._to_angle(robot_point, field_point))
robot.id = self._replace_id
robot.yellowteam = self._replace_is_yellow
robot.turnon = True
if self._invert_side:
robot.x *= -1.0
robot.y *= -1.0
robot.dir += 180
replacements = Replacements()
replacements.robots.append(robot)
self._pub_replace.publish(replacements)
self._replacement_target['robot_angle'] = False
def _replace_robot_pos(self, mouse_pos):
# ロボット位置のReplacement
field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())
# ロボット角度をradiansからdegreesに変換する
direction = 0
if self._replace_is_yellow:
direction = math.degrees(
self._robot_info['yellow'][self._replace_id].pose.theta)
else:
direction = math.degrees(
self._robot_info['blue'][self._replace_id].pose.theta)
robot = ReplaceRobot()
robot.x = field_point.x()
robot.y = field_point.y()
robot.dir = direction
robot.id = self._replace_id
robot.yellowteam = self._replace_is_yellow
robot.turnon = True
if self._invert_side:
robot.x *= -1.0
robot.y *= -1.0
robot.dir += 180
replacements = Replacements()
replacements.robots.append(robot)
self._pub_replace.publish(replacements)
self._replacement_target['robot_pos'] = False
def _replace_ball_pos(self, mouse_pos):
# ボール位置のReplacement
field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())
ball = ReplaceBall()
ball.x = field_point.x()
ball.y = field_point.y()
ball.is_enabled = True
if self._invert_side:
ball.x *= -1.0
ball.y *= -1.0
replacements = Replacements()
replacements.ball = ball
self._pub_replace.publish(replacements)
self._replacement_target['ball_pos'] = False
def _replace_ball_vel(self, mouse_pos):
# ボール速度のReplacement
ball_point = QPointF(self._ball_info.pose.x, self._ball_info.pose.y)
field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())
velocity = self._replacement_velocity(ball_point, field_point)
ball = ReplaceBall()
ball.x = ball_point.x()
ball.y = ball_point.y()
ball.vx = velocity.x()
ball.vy = velocity.y()
ball.is_enabled = True
if self._invert_side:
ball.x *= -1.0
ball.y *= -1.0
ball.vx *= -1.0
ball.vy *= -1.0
replacements = Replacements()
replacements.ball = ball
self._pub_replace.publish(replacements)
self._replacement_target['ball_vel'] = False