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 get_obstacles(self, num_list=np.array([])):
data = self.receive()
package = wrapper.SSL_WrapperPacket()
package.ParseFromString(data)
obstacles = np.array([[], []])
detection = package.detection
robots = detection.robots_blue # repeat
for robot in robots:
for num in num_list:
if robot.robot_id == num:
obstacles[0] = np.append(obstacles[0], robot.x / 1000.0)
obstacles[1] = np.append(obstacles[1], robot.y / 1000.0)
obstacles[2] = np.append(obstacles[2],
robot.orientation / 1000.0)
return obstacles[0:2]
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 main():
#multicast_group = '224.5.23.2'
server_address = ('', 10020)
# Create the socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Bind to the server address
sock.bind(server_address)
while True:
data, addr = sock.recvfrom(1024)
wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()
try:
wrapper.ParseFromString(data)
except:
pass
for robot in wrapper.detection.robots_blue:
if (robot.robot_id == 1):
print(robot.x)
print(robot.y)
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
# self.robot_info[3] = robot.vel_x/1000.0
# self.robot_info[4] = robot.vel_y/1000.0
# self.robot_info[5] = robot.rotate_vel
print('Robot', robot.confidence)
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
print(self.ball_info)
return np.array([self.robot_info[0], self.robot_info[1]])
def init(maxRobots):
global ssl_fifo, ssl_wrapper, robotsInfo
ssl_fifo = open('/tmp/ssl_fifo', 'rb')
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()
robotsInfo = np.empty([2, maxRobots, 3])
robotsInfo[:] = np.nan
def receive(self):
BUF_LENGTH=2048
buf=""
while buf is not False:
buf=self.sock.recv(BUF_LENGTH)
if buf:
packet = ssl_wrapper.SSL_WrapperPacket()
packet.ParseFromString(buf)
if packet.HasField("detection"):
detect_value_t_capture=packet.detection.t_capture
ball_info=BALL_INFO()
detect_robot=Ditection()
for robot in packet.detection.robots_blue:
robot_info=DitectMasuo()
robot_info.robot_id=robot.robot_id
robot_info.pose.x=robot.x*self.SETTING
robot_info.pose.y=robot.y*self.SETTING
robot_info.pose.theta=robot.orientation
detect_robot.blue_info.append(robot_info)
#if detect_robot.blue_info:
# self.pub_our_robot_info.publish(detect_robot)
#for robot in packet.detection.robots_yellow:
# robot_info=DitectMasuo()
# robot_info.robot_id=robot.robot_id
# robot_info.pose.x=robot.x*self.SETTING
# robot_info.pose.y=robot.y*self.SETTING
# robot_info.pose.theta=robot.orientation
# detect_robot.yellow_info.append(robot_info)
if detect_robot.yellow_info or detect_robot.blue_info:
self.pub_our_robot_info.publish(detect_robot)
for ball in packet.detection.balls:
ball_info.pose.x=ball.x*self.SETTING
ball_info.pose.y=ball.y*self.SETTING
self.pub_send2.append(self.pub_ball_info)
self.pub_send2[0].publish(ball_info)
if packet.HasField("geometry"):
geometry=GEOMETRY()
geometry.field_length=packet.geometry.field.field_length*self.SETTING
geometry.field_width=packet.geometry.field.field_width*self.SETTING
geometry.goal_width=packet.geometry.field.goal_width*self.SETTING
geometry.goal_depth=packet.geometry.field.goal_depth*self.SETTING
geometry.boundary_width=packet.geometry.field.boundary_width*self.SETTING
for line in packet.geometry.field.field_lines:
LINE=LINEINFO()
LINE.name=line.name
LINE.p1_x=line.p1.x*self.SETTING
LINE.p1_y=line.p1.y*self.SETTING
LINE.p2_x=line.p2.x*self.SETTING
LINE.p2_y=line.p2.y*self.SETTING
LINE.thickness=line.thickness
geometry.line_field.append(LINE)
for arc in packet.geometry.field.field_arcs:
ARC=ARCINFO()
ARC.name=arc.name
ARC.center_x=arc.center.x*self.SETTING
ARC.center_y=arc.center.y*self.SETTING
ARC.radius=arc.radius*self.SETTING
ARC.a1=arc.a1
ARC.a2=arc.a2
ARC.thickness=arc.thickness*self.SETTING
geometry.arc_field.append(ARC)
self.pub_send_geometry.publish(geometry)
if __name__ == '__main__':
rospy.init_node('grsim_ros_bridge', anonymous=False)
multicast_if_addr = '192.168.3.9'
multicast_group = '224.5.23.2'
multicast_port = 10020
my_addr = '0.0.0.0'
server_address = (my_addr, multicast_port)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(server_address)
mreq = socket.inet_aton(multicast_group) + socket.inet_aton(
multicast_if_addr)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
#make protobuf instance
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()
#make publisher
field_objects_pub = rospy.Publisher('/field_objects',
FieldObjects,
queue_size=1)
field_objects_msg = FieldObjects()
broadcaster = tf2_ros.TransformBroadcaster()
while not rospy.is_shutdown():
data = sock.recv(1024)
try:
ssl_wrapper.ParseFromString(data)
field_objects_msg = create_field_objects_message(
ssl_wrapper, broadcaster)
field_objects_pub.publish(field_objects_msg)
except google.protobuf.message.DecodeError:
pass
