def test_convert_protobuf(self):
# Test Packet data
packet = wrapper.SSL_WrapperPacket()
detection = packet.detection
detection.frame_number = 0
detection.t_capture = t_capture = 1.1
detection.t_sent = t_sent = 1.2
detection.camera_id = camera_id = 2
# Ball data
ball = detection.balls.add()
ball.confidence = 0.0
ball.x = ball_x = 1.0
ball.y = ball_y = 2.0
ball.pixel_x = 0
ball.pixel_y = 0
# Robot data
robot = detection.robots_blue.add()
robot.confidence = 0.0
robot.robot_id = robot_id = 13
robot.x = robot_x = 3.0
robot.y = robot_y = 4.0
robot.orientation = orientation = math.pi
robot.pixel_x = 0
robot.pixel_y = 0
message = packet.SerializeToString()
converter = FormatConverter(friend_color='blue', do_side_invert=False)
converter.convert_protobuf(message)
ball_packet = converter.get_ball_packet()
friend_packets = converter.get_friend_packets()
# convert unit
TO_METER = 0.001
ball_x *= TO_METER
ball_y *= TO_METER
robot_x *= TO_METER
robot_y *= TO_METER
self.assertAlmostEqual(ball_packet.data[0].t_capture, t_capture)
self.assertAlmostEqual(ball_packet.data[0].t_sent, t_sent)
self.assertAlmostEqual(ball_packet.data[0].camera_id, camera_id)
self.assertAlmostEqual(ball_packet.data[0].pose.position.x, ball_x)
self.assertAlmostEqual(ball_packet.data[0].pose.position.y, ball_y)
for packet in friend_packets:
if (packet.data):
self.assertAlmostEqual(packet.robot_id, robot_id)
self.assertAlmostEqual(packet.data[0].t_capture, t_capture)
self.assertAlmostEqual(packet.data[0].t_sent, t_sent)
self.assertAlmostEqual(packet.data[0].camera_id, camera_id)
self.assertAlmostEqual(packet.data[0].pose.position.x, robot_x)
self.assertAlmostEqual(packet.data[0].pose.position.y, robot_y)
def recvData(self):
global wc
wp = wrapper.SSL_WrapperPacket()
while playAll:
wp.ParseFromString(sock.recv(65536))
if wp.detection.IsInitialized():
debugP("Frame number: {}".format(
type(wp.detection.frame_number)))
debugP("Time Capture: {}".format(wp.detection.t_capture))
debugP("Time Sent: {}".format(wp.detection.t_sent))
debugP("Camera ID: {}".format(wp.detection.camera_id))
if len(wc.teams) == 0:
wc.teams.append(wp.detection.robots_yellow)
wc.teams.append(wp.detection.robots_blue)
else:
wc.teams[0] = (wp.detection.robots_yellow)
wc.teams[1] = (wp.detection.robots_blue)
for i in wp.detection.balls:
#Assuming that there is only one ball.
#The last one will overwrite the other ones.
wc.ball = i
debugP("Ball")
debugP("\tConfidence: {}".format(i.confidence))
debugP("\tx: {}".format(i.x))
debugP("\ty: {}".format(i.y))
debugP("\tz: {}".format(i.z))
for i in wp.detection.robots_yellow:
debugP("Robot Yellow {}".format(i.robot_id))
debugP("\tConfidence: {}".format(i.confidence))
debugP("\tx: {}".format(i.x))
debugP("\ty: {}".format(i.y))
debugP("\tOrientation: {}".format(i.orientation))
for i in wp.detection.robots_blue:
debugP("Robot Blue {}".format(i.robot_id))
debugP("\tConfidence: {}".format(i.confidence))
debugP("\tx: {}".format(i.x))
debugP("\ty: {}".format(i.y))
debugP("\tOrientation: {}".format(i.orientation))
#debugP (wp.detection)
pass
if wp.geometry.IsInitialized():
debugP(wp.geometry)
wc.geo = wp.geometry
pass
debugP("************")
self.updated.emit()
def get_yellow_info(self):
data = self.receive()
package = wrapper.SSL_WrapperPacket()
package.ParseFromString(data)
detection = package.detection
robots = detection.robots_blue # repeat
self.robots = [[robot.x, robot.y, robot.orientation] for robot in
robots]
balls = detection.balls # not repeat
for ball in balls:
self.ball_info[0] = ball.x / 1000.0
self.ball_info[1] = ball.y / 1000.0
def convert_protobuf(self, protobuf_binary):
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()
ssl_wrapper.ParseFromString(protobuf_binary)
t_capture = ssl_wrapper.detection.t_capture
t_sent = ssl_wrapper.detection.t_sent
camera_id = ssl_wrapper.detection.camera_id
for ball in ssl_wrapper.detection.balls:
data = VisionData()
data.t_capture = t_capture
data.t_sent = t_sent
data.camera_id = camera_id
data.pose = self._vision_ball_to_pose(
ball.x, ball.y, ball.z)
self._ball_packet.data.append(data)
friend_robots = []
enemy_robots = []
if self.friend_color == 'blue':
friend_robots = ssl_wrapper.detection.robots_blue
enemy_robots = ssl_wrapper.detection.robots_yellow
else:
friend_robots = ssl_wrapper.detection.robots_yellow
enemy_robots = ssl_wrapper.detection.robots_blue
for robot in friend_robots:
data = VisionData()
data.t_capture = t_capture
data.t_sent = t_sent
data.camera_id = camera_id
data.pose = self._vision_robot_to_pose(
robot.x, robot.y, robot.height, robot.orientation)
robot_id = robot.robot_id
self._friend_packets[robot_id].data.append(data)
for robot in enemy_robots:
data = VisionData()
data.t_capture = t_capture
data.t_sent = t_sent
data.camera_id = camera_id
data.pose = self._vision_robot_to_pose(
robot.x, robot.y, robot.height, robot.orientation)
robot_id = robot.robot_id
self._enemy_packets[robot_id].data.append(data)
def get_info(self, ROBOT_ID):
data = self.receive()
package = wrapper.SSL_WrapperPacket()
package.ParseFromString(data)
detection = package.detection
robots = detection.robots_blue # repeat
for robot in robots:
if robot.robot_id == ROBOT_ID:
self.robot_info[0] = robot.x/1000.0
self.robot_info[1] = robot.y/1000.0
self.robot_info[2] = robot.orientation
balls = detection.balls # not repeat
for ball in balls:
self.ball_info[0] = ball.x/1000.0
self.ball_info[1] = ball.y/1000.0
def _receive_geometry(self, buf):
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()
ssl_wrapper.ParseFromString(buf)
if ssl_wrapper.HasField('geometry'):
field_size = GeometryFieldSize()
field = ssl_wrapper.geometry.field
# SSL_geometry uses meter unit
# CON-SAI uses milli meters
field_size.field_length = field.field_length * self._TO_METER
field_size.field_width = field.field_width * self._TO_METER
field_size.goal_width = field.goal_width * self._TO_METER
field_size.goal_depth = field.goal_depth * self._TO_METER
field_size.boundary_width = field.boundary_width * self._TO_METER
for line in field.field_lines:
line_segment = FieldLineSegment()
line_segment.name = line.name
line_segment.p1_x = line.p1.x * self._TO_METER
line_segment.p1_y = line.p1.y * self._TO_METER
line_segment.p2_x = line.p2.x * self._TO_METER
line_segment.p2_y = line.p2.y * self._TO_METER
line_segment.thickness = line.thickness * self._TO_METER
field_size.field_lines.append(line_segment)
for arc in field.field_arcs:
circular_arc = FieldCircularArc()
circular_arc.name = arc.name
circular_arc.center_x = arc.center.x * self._TO_METER
circular_arc.center_y = arc.center.y * self._TO_METER
circular_arc.radius = arc.radius * self._TO_METER
circular_arc.a1 = arc.a1
circular_arc.a2 = arc.a2
circular_arc.thickness = arc.thickness * self._TO_METER
field_size.field_arcs.append(circular_arc)
self._geometry_field_size_publisher.publish(field_size)
def get_info(self, ROBOT_ID):
data, robot_Data = self.receive()
package = messages_wrapper.SSL_WrapperPacket()
try:
package.ParseFromString(data)
detection = package.detection
#print('camera id:', detection.camera_id)
robots = detection.robots_blue # repeat
robot_max_conf = 0
for robot in robots:
if robot.robot_id == ROBOT_ID and robot.confidence > robot_max_conf:
self.robot_info[0] = robot.x / 1000.0
self.robot_info[1] = robot.y / 1000.0
self.robot_info[2] = robot.orientation
#print('Robot', robot.confidence)
balls = detection.balls # repeat
ball_max_conf = 0
for ball in balls:
if ball.confidence >= ball_max_conf:
self.ball_info[0] = ball.x / 1000.0
self.ball_info[1] = ball.y / 1000.0
except:
print("vision error")
pass
#print('Ball', ball.confidence)
package = zss.Robot_Status
try:
package.ParseFromString(robot_Data)
for robot in package:
if robot.robot_id == ROBOT_ID:
self.robot_info[3] = robot.indrared
except:
print("status error")
pass
import struct
import proto.messages_robocup_ssl_wrapper_pb2 as wrapper
MCAST_GRP = '224.5.23.2'
MCAST_PORT = 10020
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.bind(('', MCAST_PORT)) # use MCAST_GRP instead of '' to listen only
# to MCAST_GRP, not all groups on MCAST_PORT
mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
wp = wrapper.SSL_WrapperPacket()
while True:
wp.ParseFromString(sock.recv(65536))
if wp.detection.IsInitialized():
print("Frame number:", wp.detection.frame_number)
print("Time Capture:", wp.detection.t_capture)
print("Time Sent:", wp.detection.t_sent)
print("Camera ID:", wp.detection.camera_id)
for i in wp.detection.balls:
print("Ball")
print("\tConfidence:", i.confidence)
print("\tx:", i.x)
print("\ty:", i.y)
print("\tz:", i.z)
for i in wp.detection.robots_yellow: