def __init__(self, context): super(MasterInfo, self).__init__(context) self.initialised = False self.setObjectName('MasterInfo') self._current_dotcode = None self._master_info = rocon_master_info.get_master_info(0.5) # at worst a small timeout here, but perhaps should be run in a thread. self._widget = QWidget() self._widget.setObjectName('RoconMasterInfoUi') rospack = rospkg.RosPack() ui_file = os.path.join(rospack.get_path('rocon_qt_master_info'), 'ui', 'master_info.ui') loadUi(ui_file, self._widget) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) pixmap = QPixmap() pixmap.loadFromData(self._master_info.icon.data, format=self._master_info.icon.format) self._widget.icon_label.setPixmap(pixmap) self._widget.icon_label.resize(pixmap.width(), pixmap.height()) self._widget.info_label.resize(200, pixmap.height()) self._widget.info_label.setText("<b>Name:</b> %s<br/><b>Rocon Version:</b> %s<br/><b>Description:</b> %s" % (self._master_info.name, self._master_info.version, self._master_info.description)) self._widget.adjustSize() context.add_widget(self._widget)
def __init__(self, context): super(ServiceInfo, self).__init__(context) self.initialised = False self.setObjectName('ServiceInfo') self._current_dotcode = None counter = 0 service_info = concert_service_utilities.service_information.get_services_info() dict = service_info[1] #dict = {} nameList = service_info[0] #nameList = [] # Create Widget self._widget = QWidget() self._widget.setObjectName('ConcertServiceInfoUi') rospack = rospkg.RosPack() ui_file = os.path.join(rospack.get_path('concert_qt_service_info'), 'ui', 'concert_service_info.ui') loadUi(ui_file, self._widget) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) for value in nameList: info_text = "<html>" info_text += "<p><b>Resource Name: </b>" + dict[value].resource_name + "</p>" info_text += "<p><b>Name: </b>" + dict[value].name + "</p>" info_text += "<p><b>Description: </b>" + dict[value].description + "</p>" info_text += "<p><b>Author: </b>" + dict[value].author + "</p>" info_text += "<p><b>Priority: </b>" + str(dict[value].priority) + "</p>" info_text += "<p><b>Launcher: </b>" + dict[value].launcher_type + "</p>" info_text += "<p><b>Status: </b>" + str(dict[value].status) + "</p>" info_text += "<p><b>Enabled: </b>" + str(dict[value].enabled) + "</p>" info_text += "<p><b>Icon: </b>" + str(dict[value].icon.resource_name) + "</p>" info_text += "</html>" if counter == 0: self._widget.app_info.appendHtml(info_text) pixmap2 = QPixmap() pixmap2.loadFromData(dict[value].icon.data, format=dict[value].icon.format) if QPixmap.isNull(pixmap2): icon_file2 = os.path.join(rospack.get_path('concert_qt_service_info'), 'resources', 'images', 'unknown.png') pixmap2 = QPixmap(icon_file2, format="png") self._widget.app_icon2.setPixmap(pixmap2) self._widget.app_icon2.resize(pixmap2.width(), pixmap2.height()) if not QPixmap.isNull(pixmap2): self._widget.app_icon2.setPixmap(pixmap2) self._widget.app_icon2.resize(pixmap2.width(), pixmap2.height()) if counter == 1: self._widget.app_info2.appendHtml(info_text) pixmap = QPixmap() pixmap.loadFromData(dict[value].icon.data, format=dict[value].icon.format) if QPixmap.isNull(pixmap): icon_file = os.path.join(rospack.get_path('concert_qt_service_info'), 'resources', 'images', 'unknown.png') pixmap = QPixmap(icon_file, format="png") self._widget.app_icon.setPixmap(pixmap) self._widget.app_icon.resize(pixmap.width(), pixmap.height()) if not QPixmap.isNull(pixmap): self._widget.app_icon.setPixmap(pixmap) self._widget.app_icon.resize(pixmap.width(), pixmap.height()) counter = counter + 1 context.add_widget(self._widget)
def _set_icon(self): pixmap = QPixmap() pixmap.loadFromData( self.qt_rapp_manager_info._get_icon().data, format=self.qt_rapp_manager_info._get_icon().format) self._widget.icon_label.setPixmap(pixmap) self._widget.icon_label.resize(pixmap.width(), pixmap.height())
def _display_image(self): image = self.teleop_app_info.image_data if image: if len(self.scene.items()) > 1: self.scene.removeItem(self.scene.items()[0]) image_data = self.teleop_app_info.image_data.data pixmap = QPixmap() pixmap.loadFromData( image_data, format="PNG", ) self._widget.camera_view.fitInView( QRectF(0, 0, pixmap.width(), pixmap.height()), Qt.KeepAspectRatio) self.scene.addPixmap(pixmap) self.scene.update() else: self.scene.clear()
class SingleField: def __init__(self, gameFrame, fieldImageLabel): self.frame = gameFrame self.field = fieldImageLabel rp = rospkg.RosPack() ip_filename = os.path.join(rp.get_path('bitbots_live_tool_rqt'), 'resource', 'ip_config.yaml') with open(ip_filename, "r") as file: config = yaml.load(file) self.fieldfilename = config.get("FIELD_IMAGE") self.field_scale_global = config.get("FIELD_SCALE") self.default_positions = config.get("DEFAULT_POSITIONS") file.close() print("Config: " + self.fieldfilename + ", " + str(self.field_scale_global)) rp = rospkg.RosPack() self.fieldfilename = os.path.join( BallPool.rp.get_path('bitbots_live_tool_rqt'), 'resource', self.fieldfilename) self.fieldPixmap = QPixmap(self.fieldfilename) self.field.setPixmap(self.fieldPixmap) field_width = self.fieldPixmap.width() field_height = self.fieldPixmap.height() self.field_aspect = float(field_width) / float(field_height) self.frame_aspect = float(self.frame.width()) / float( self.frame.height()) self.field_border = 40 # margin to frame self.transform = QTransform() self.fieldIsSwitched = False self.icon_timeout = 3 self.team_colors = {3: "cyan", 2: "magenta"} if self.field_aspect >= self.frame_aspect: # self.field_size_x = self.frame.width() - self.field_border * 2 self.field_size_y = self.field_size_x / self.field_aspect else: self.field_size_y = self.frame.height() - self.field_border * 2 self.field_size_x = self.field_size_y / self.field_aspect self.field.setScaledContents(True) self.field.setFixedSize(self.field_size_x, self.field_size_y) self.field_scale = float(self.field_size_x) / float(field_width) #print(self.field_scale) self.screenMidX = int(self.frame.width() / 2) self.screenMidY = int(self.frame.height() / 2) self.field.move(self.screenMidX - int(self.field.width() / 2), self.screenMidY - int(self.field.height() / 2)) self.colors = ["red", "green", "yellow", "blue"] self.robots = {} # dict where keys are robot IDs self.tabToRobot = {} # dict where keys are tab-nrs [0-3] self.ball_pool = BallPool(self.frame, size=42) self.rob_pool = RobotPool(self.frame, size=48) self.opp_pool = OpponentPool(self.frame, size=42) self.team_pool = TeammatePool(self.frame, size=42) self.cross_pool = CrossPool(self.frame, size=34) self.undef_pool = UndefinedPool(self.frame, size=42) # logic def setSide(self, default): self.fieldIsSwitched = not default # adds new robot, color is taken automatically. Color can be given manually def addRobot(self, robotID, color=""): if color == "" or color not in self.colors: if len(self.colors) > 0: color = self.colors.pop() else: color = "red" rob_info = RobotInformation(robotID, self.frame, color) rob_info.rob_label = self.rob_pool.getRobotLabel(color) rob_info.pixmap = rob_info.rob_label.pixmap( ) # save original pixmap for rotation rob_info.rob_label.update() rob_info.ang_label = AngularLabel(self.frame) rob_info.arrow_label = Arrowlabel(self.frame) self.frame.update() self.robots[robotID] = rob_info self.tabToRobot[len(self.tabToRobot)] = rob_info defpos = {"x": 0, "y": 0} if len(self.default_positions) > 0: defpos = self.default_positions.pop() self.setRobotPosition(robotID, defpos.get("x"), defpos.get("y"), 0) # sets default position """ if self.robotsVisible: rob_info.rob_label.show() else: rob_info.rob_label.hide()""" rob_info.rob_label.show() rob_info.ang_label.show() rob_info.arrow_label.show() print(self.tabToRobot) print(self.robots) self.upadteAllVisiblities(rob_info) return rob_info def _meter_to_UI(self, val): return (val / self.field_scale_global) * self.field_scale # sets the robots position in meter!!!! def setRobotPosition(self, robotID, x, y, angle): if self.robots.has_key(robotID): rob = self.robots.get(robotID) if x != None: if self.fieldIsSwitched: rob.x = -x else: rob.x = x if y != None: if self.fieldIsSwitched: rob.y = -y else: rob.y = y if angle != None: if self.fieldIsSwitched: rob.angel = -angle + self.degToRadians(180) else: rob.angel = -angle transform = QTransform() transform.rotateRadians(rob.angel) rotPixMap = QPixmap( rob.rob_label.originalPixmap).transformed(transform) rob.rob_label.setPixmap(rotPixMap) rob.rob_label.setScaledContents(True) addScale = (abs(math.sin(rob.angel * 2))) * (math.sqrt(2) - 1) newsize = self.rob_pool.size + (self.rob_pool.size * addScale) rob.rob_label.setFixedSize(newsize, newsize) #print(addScale, newsize, rob.rob_label.width()) rob.rob_label.setScaledContents(True) rob.rob_label.move(self.screenMidX - int(rob.rob_label.width() / 2) + self._meter_to_UI(rob.x), \ self.screenMidY - int(rob.rob_label.height() / 2) - self._meter_to_UI(rob.y)) rob.ang_label.move( rob.rob_label.x() + int(rob.rob_label.width() / 2) - rob.ang_label.width() / 2,\ rob.rob_label.y() + int(rob.rob_label.height() / 2) - rob.ang_label.height() / 2) rob.arrow_label.move(rob.rob_label.x() + int(rob.rob_label.width() / 2) - rob.arrow_label.width() / 2,\ rob.rob_label.y() + int(rob.rob_label.height() / 2) - rob.arrow_label.height() / 2) rob.arrow_label.setRoboAngle(self.radToDeg(rob.angel)) rob.ang_label.setAngles(self.radToDeg(-rob.angel), None) #rob.rob_label.update() #rob.rob_label.repaint() def setBallsFor(self, robotID, listPositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentBallLabel)): self.ball_pool.returnBallLabel(rob.currentBallLabel.pop(), rob.color) for pos in listPositions: if time_secs - lastUpdate < self.icon_timeout: bpx, bpy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) ball = self.ball_pool.getBallLabel(rob.color) ball.move(self.screenMidX - int(ball.width() / 2) + self._meter_to_UI(bpx), \ self.screenMidY - int(ball.height() / 2) + self._meter_to_UI(bpy)) rob.currentBallLabel.append(ball) self.upadteAllVisiblities(rob) def setPathsFor(self, robotID, listPaths): if self.robots.has_key(robotID): rob = self.robots.get(robotID) for i in range(len(rob.currentPathLabel)): self.cross_pool.returnCrossLabel(rob.currentPathLabel.pop(), rob.color) for pos in listPaths: cross = self.cross_pool.getCrossLabel(rob.color) if self.fieldIsSwitched: cross.move(self.screenMidX - int(cross.width() / 2) - self._meter_to_UI(pos[0]), \ self.screenMidY - int(cross.height() / 2) - self._meter_to_UI(pos[1])) else: cross.move(self.screenMidX - int(cross.width() / 2) + self._meter_to_UI(pos[0]), \ self.screenMidY - int(cross.height() / 2) + self._meter_to_UI(pos[1])) cross.show() rob.currentPathLabel.append(cross) self.upadteAllVisiblities(rob) def setTeammatesFor(self, robotID, listPositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentTeammateLabel)): self.team_pool.returnTeammateLabel( rob.currentTeammateLabel.pop(), rob.color) for pos in listPositions: if time_secs - lastUpdate < self.icon_timeout: bpx, bpy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) teamm = self.team_pool.getTeammateLabel(rob.color) teamm.move(self.screenMidX - int(teamm.width() / 2) + self._meter_to_UI(bpx), \ self.screenMidY - int(teamm.height() / 2) + self._meter_to_UI(bpy)) teamm.show() rob.currentTeammateLabel.append(teamm) self.upadteAllVisiblities(rob) def setOpponentsFor(self, robotID, listPositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentOpponentsLabel)): self.opp_pool.returnOpponentLabel( rob.currentOpponentsLabel.pop(), rob.color) for pos in listPositions: if time_secs - lastUpdate < self.icon_timeout: bpx, bpy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) opp = self.opp_pool.getOpponentLabel(rob.color) opp.move(self.screenMidX - int(opp.width() / 2) + self._meter_to_UI(bpx), \ self.screenMidY - int(opp.height() / 2) + self._meter_to_UI(bpy)) opp.show() rob.currentOpponentsLabel.append(opp) self.upadteAllVisiblities(rob) # obstacles are undefined def setObstaclesFor(self, robotID, listObstacles, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentObstacleLabel)): self.undef_pool.returnUndefLabel( rob.currentObstacleLabel.pop(), rob.color) for pos in listObstacles: if time_secs - lastUpdate < self.icon_timeout: bpx, bpy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) obs = self.undef_pool.getUndefLabel(rob.color) obs.move(self.screenMidX - int(obs.width() / 2) + self._meter_to_UI(bpx), \ self.screenMidY - int(obs.height() / 2) + self._meter_to_UI(bpy)) obs.show() rob.currentObstacleLabel.append(obs) self.upadteAllVisiblities(rob) # Message decoding ============================================================================== def setTrajectoryMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) # simple absolute target position with target angle if data.has_key(TrajectoryMsg.label_moveToX): self.setPathsFor(robotID, [(data.get(TrajectoryMsg.label_moveToX), data.get(TrajectoryMsg.label_moveToY), data.get(TrajectoryMsg.label_finalAngle))]) # Twist vectors: rotation if data.has_key(TrajectoryMsg.label_rotateVel): rob = self.robots.get(robotID) rob.ang_label.setAngles( self.radToDeg(-rob.angel), self.radToDeg(data.get(TrajectoryMsg.label_rotateVel))) # Twist vectors: linear velocity if data.has_key(TrajectoryMsg.label_moveVelX): rob = self.robots.get(robotID) rob.arrow_label.setLinearAngle( self._meter_to_UI(data.get(TrajectoryMsg.label_moveVelX)), self._meter_to_UI(data.get(TrajectoryMsg.label_moveVelY))) rob.arrow_label.move(rob.rob_label.x() + int(rob.rob_label.width() / 2) - rob.arrow_label.width() / 2, \ rob.rob_label.y() + int(rob.rob_label.height() / 2) - rob.arrow_label.height() / 2) rob.arrow_label.setRoboAngle(self.radToDeg(rob.angel)) def setDetectionMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) rob = self.robots.get(robotID) if data.has_key(DetectionMsg.label_ball_info): self.setBallsFor(robotID, [(data.get(DetectionMsg.label_ball_info).get("x"), data.get(DetectionMsg.label_ball_info).get("y"))], \ data.get(DetectionMsg.label_ball_info).get(Name.last_update)) if data.has_key(DetectionMsg.label_obstacles): lsUndef = [] lsTeammate = [] lsOpponent = [] for ob in data.get(DetectionMsg.label_obstacles): ob_color = ob.get(DetectionMsg.label_obstacle_info).get( DetectionMsg.label_color) if ob_color == rob.team_color: #mitspieler +2 weil die farben lsTeammate.append([ ob.get(DetectionMsg.label_obstacle_pos).get("x"), ob.get(DetectionMsg.label_obstacle_pos).get("y") ]) elif ob_color == rob.oppo_color: #gegner lsOpponent.append([ ob.get(DetectionMsg.label_obstacle_pos).get("x"), ob.get(DetectionMsg.label_obstacle_pos).get("y") ]) else: #undefined lsUndef.append([ ob.get(DetectionMsg.label_obstacle_pos).get("x"), ob.get(DetectionMsg.label_obstacle_pos).get("y") ]) self.setObstaclesFor( robotID, lsUndef, data.get(DetectionMsg.label_last_obstacle_update)) self.setTeammatesFor( robotID, lsTeammate, data.get(DetectionMsg.label_last_obstacle_update)) self.setOpponentsFor( robotID, lsOpponent, data.get(DetectionMsg.label_last_obstacle_update)) def setPositionMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) print("add robot: " + robotID) if (data.has_key(PositionMsg.label_pos)): self.setRobotPosition(robotID, data.get(PositionMsg.label_pos).get("x"), data.get(PositionMsg.label_pos).get("y"),\ data.get(PositionMsg.label_orientation).get(PositionMsg.label_yaw)) def setStatusMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) print("add robot: " + robotID) if data.has_key(StatusMsg.labelTeamColor): rob = self.robots.get(robotID) statusColor = data.get(StatusMsg.labelTeamColor) statusColor = ( 1 - statusColor ) + 2 # /gamestate und /obstacles have different constants for colors oppoColor = data.get(StatusMsg.labelTeamColor) + 2 rob.team_color = statusColor rob.oppo_color = oppoColor #print(rob.team_color, rob.oppo_color) # visibility =================================================================================== # hides all of the tabs components if param hide is true # num is the tabs number #def hideAll(self, hide, num): # Wird aber momentan ueber information_tab geregelt def changeVisibilityFor(self, ls, visible): for e in ls: if visible: e.show() else: e.hide() def upadteAllVisiblities(self, rob): self.changeVisibilityFor(rob.currentTeammateLabel, rob.teammateVisible) self.changeVisibilityFor(rob.currentObstacleLabel, rob.obstacleVisible) self.changeVisibilityFor(rob.currentOpponentsLabel, rob.opponentVisible) self.changeVisibilityFor(rob.currentBallLabel, rob.ballVisible) self.changeVisibilityFor(rob.currentPathLabel, rob.pathVisible) def setOwnRobotsVisibility(self, visible, num): if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) if visible: rob.rob_label.show() rob.ang_label.show() rob.arrow_label.show() else: rob.rob_label.hide() rob.ang_label.hide() rob.arrow_label.hide() def setTeammateVisibility(self, visible, num): if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) rob.teammateVisible = visible self.upadteAllVisiblities(rob) def setOpponentVisibility(self, visible, num): if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) rob.opponentVisible = visible self.upadteAllVisiblities(rob) def setPathVisibility(self, visible, num): if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) rob.pathVisible = visible self.upadteAllVisiblities(rob) """ def setObstacleVisibility(self, visible, num): if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) rob.obstacleVisible = visible self.upadteAllVisiblities(rob) """ def setBallVisibility(self, visible, num): #print(num) #print(self.tabToRobot) if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) rob.ballVisible = visible #print("ball visible: " + str(rob.ballVisible)) self.upadteAllVisiblities(rob) def setUndefVisibility(self, visible, num): if self.tabToRobot.has_key(num): rob = self.tabToRobot.get(num) rob.obstacleVisible = visible self.upadteAllVisiblities(rob) # Helper =================================================================== def vec_rotate(self, x, y, angle_rad): xneu = x * math.cos(angle_rad) - y * math.sin(angle_rad) yneu = y * math.cos(angle_rad) + x * math.sin(angle_rad) return [xneu, yneu] def relToAbs(self, fromx, fromy, fromAng, relx, rely): rx, ry = self.vec_rotate(relx, rely, fromAng) return (fromx + rx, fromy + ry) def radToDeg(self, rads): return rads * 57.29578 def degToRadians(self, deg): return deg / 57.29578
class QuarterField: # fieldframe = frame, which contains game-field and all information of that roboter def __init__(self, gameframe, fieldImageLabel): # receive parameters self.frame = gameframe self.field = fieldImageLabel # set yaml config file rp = rospkg.RosPack() ip_filename = os.path.join(rp.get_path('bitbots_live_tool_rqt'), 'resource', 'ip_config.yaml') with open(ip_filename, "r") as file: config = yaml.load(file) self.fieldfilename = config.get("FIELD_IMAGE") self.field_scale_global = config.get("FIELD_SCALE") self.default_positions = config.get("DEFAULT_POSITIONS") file.close() # debug #print("Config: " + self.fieldfilename + ", " + str(self.field_scale_global)) # set & scale field ==================================================== rp = rospkg.RosPack() self.fieldfilename = os.path.join( BallPool.rp.get_path('bitbots_live_tool_rqt'), 'resource', self.fieldfilename) self.fieldPixmap = QPixmap(self.fieldfilename) self.field.setPixmap(self.fieldPixmap) field_width = self.fieldPixmap.width() field_height = self.fieldPixmap.height() self.field_aspect = float(field_width) / float(field_height) self.frame_aspect = float(self.frame.width()) / float( self.frame.height()) self.field_border = 10 # margin to frame self.transform = QTransform() self.icon_timeout = 3 self.fieldIsSwitched = False # for function "switch sides" if self.field_aspect >= self.frame_aspect: self.field_size_x = self.frame.width() - self.field_border * 2 self.field_size_y = self.field_size_x / self.field_aspect else: self.field_size_y = self.frame.height() - self.field_border * 2 self.field_size_x = self.field_size_y / self.field_aspect self.field.setScaledContents(True) self.field.setFixedSize(self.field_size_x, self.field_size_y) self.field_scale = float(self.field_size_x) / float(field_width) #print(self.field_scale) self.screenMidX = int(self.frame.width() / 2) self.screenMidY = int(self.frame.height() / 2) self.field.move(self.screenMidX - int(self.field.width() / 2), self.screenMidY - int(self.field.height() / 2)) # ================================================================================= self.colors = ["red", "green", "yellow", "blue"] self.team_colors = {3: "cyan", 2: "magenta"} self.op_color = None self.mate_color = None self.own_color = None self.myRobo_color = None self.robots = {} # dict where keys are robot IDs self.ball_pool = BallPool(self.frame, size=32) self.rob_pool = RobotPool(self.frame, size=36) self.opp_pool = OpponentPool(self.frame, size=32) self.team_pool = TeammatePool(self.frame, size=32) self.cross_pool = CrossPool(self.frame, size=17) self.undef_pool = UndefinedPool(self.frame, size=32) #self.cyanmagenta_pool = CyanMagentaPool(self.frame, size=26) #rob = self.rob_pool.getRobotLabel("green") # farbe nur zum testen, sonst uebergabe einer Farbe #self.rob_pool.returnRobotLabel(rob, ("green")) def setSide(self, default): self.fieldIsSwitched = not default # helping method for convert meters to UI-values def _meter_to_UI(self, val): return (val / self.field_scale_global) * self.field_scale # add the roboter def addRobot(self, robotID, color=""): if color == "": color = self.colors.pop() rob_info = RobotInformation(robotID, self.frame, color) rob_info.rob_label = self.rob_pool.getRobotLabel(color) rob_info.pixmap = rob_info.rob_label.pixmap( ) # saving original pixmap for rotation rob_info.rob_label.update() rob_info.ang_label = AngularLabel(self.frame) rob_info.arrow_label = Arrowlabel(self.frame) self.frame.update() self.robots[robotID] = rob_info defpos = {"x": 0, "y": 0} if len(self.default_positions) > 0: defpos = self.default_positions.pop() self.setRobotPosition(robotID, defpos.get("x"), defpos.get("y"), 0) # sets default position # sets the robots position in meter!!!! def setRobotPosition(self, robotID, x, y, angle): if self.robots.has_key(robotID): rob = self.robots.get(robotID) if x != None: if self.fieldIsSwitched: rob.x = -x else: rob.x = x if y != None: if self.fieldIsSwitched: rob.y = -y else: rob.y = y if angle != None: if self.fieldIsSwitched: rob.angel = -angle + self.degToRadians(180) else: rob.angel = -angle transform = QTransform() transform.rotateRadians(rob.angel) rotPixMap = QPixmap( rob.rob_label.originalPixmap).transformed(transform) rob.rob_label.setPixmap(rotPixMap) rob.rob_label.setScaledContents(True) addScale = (abs(math.sin(rob.angel * 2))) * (math.sqrt(2) - 1) newsize = self.rob_pool.size + (self.rob_pool.size * addScale) rob.rob_label.setFixedSize(newsize, newsize) rob.rob_label.setScaledContents(True) rob.rob_label.move(self.screenMidX - int(rob.rob_label.width() / 2) + self._meter_to_UI(rob.x), \ self.screenMidY - int(rob.rob_label.height() / 2) - self._meter_to_UI(rob.y)) rob.ang_label.move( rob.rob_label.x() + int(rob.rob_label.width() / 2) - rob.ang_label.width() / 2,\ rob.rob_label.y() + int(rob.rob_label.height() / 2) - rob.ang_label.height() / 2) rob.ang_label.setAngles(self.radToDeg(-rob.angel), None) rob.arrow_label.move(rob.rob_label.x() + int(rob.rob_label.width() / 2) - rob.arrow_label.width() / 2, \ rob.rob_label.y() + int(rob.rob_label.height() / 2) - rob.arrow_label.height() / 2) rob.arrow_label.setRoboAngle(self.radToDeg(rob.angel)) #rob.rob_label.update() #rob.rob_label.repaint() def setPathsFor(self, robotID, listPaths): if self.robots.has_key(robotID): rob = self.robots.get(robotID) for i in range(len(rob.currentPathLabel)): self.cross_pool.returnCrossLabel(rob.currentPathLabel.pop(), rob.color) for pos in listPaths: cross = self.cross_pool.getCrossLabel(rob.color) cross.move(self.screenMidX - int(cross.width() / 2) + self._meter_to_UI(pos[0]), \ self.screenMidY - int(cross.height() / 2) + self._meter_to_UI(pos[1])) cross.show() rob.currentPathLabel.append(cross) # receive and set message data, cyan & magenta for mates def setTeammatePosition(self, robotID, listMatePositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentTeammateLabel)): self.team_pool.returnTeammateLabel( rob.currentTeammateLabel.pop(), rob.color) for pos in listMatePositions: if time_secs - lastUpdate < self.icon_timeout: tpx, tpy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) #mate = self.team_pool.getTeammateLabel(pos[2])# Index 2 = always cyan OR magenta mate = self.team_pool.getTeammateLabel( self.team_colors.get(rob.team_color) ) # Index 2 = always cyan OR magenta mate.move(self.screenMidX - int(mate.width() / 2) + self._meter_to_UI(tpx), \ self.screenMidY - int(mate.height() / 2) + self._meter_to_UI(tpy)) mate.show() rob.currentTeammateLabel.append(mate) def setOpponentPosition(self, robotID, listOpponentPositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentOpponentsLabel)): self.opp_pool.returnOpponentLabel( rob.currentOpponentsLabel.pop(), rob.color) for pos in listOpponentPositions: if time_secs - lastUpdate < self.icon_timeout: oppx, oppy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) #op = self.opp_pool.getOpponentLabel(pos[2]) op = self.opp_pool.getOpponentLabel( self.team_colors.get(rob.oppo_color)) op.move(self.screenMidX - int(op.width() / 2) + self._meter_to_UI(oppx), \ self.screenMidY - int(op.height() / 2) + self._meter_to_UI(oppy)) op.show() rob.currentOpponentsLabel.append(op) # obstacles are undefined def setObstaclePosition(self, robotID, listObstaclePositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentObstacleLabel)): self.undef_pool.returnUndefLabel( rob.currentObstacleLabel.pop(), rob.color) for pos in listObstaclePositions: if time_secs - lastUpdate < self.icon_timeout: opx, opy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) obstacle = self.undef_pool.getUndefLabel("grey") obstacle.move(self.screenMidX - int(obstacle.width() / 2) + self._meter_to_UI(opx), \ self.screenMidY - int(obstacle.height() / 2) + self._meter_to_UI(opy)) obstacle.show() rob.currentObstacleLabel.append(obstacle) def setBallsFor(self, robotID, listPositions, lastUpdate): if self.robots.has_key(robotID): rob = self.robots.get(robotID) time_secs = rospy.get_rostime().secs for i in range(len(rob.currentBallLabel)): self.ball_pool.returnBallLabel(rob.currentBallLabel.pop(), rob.color) for pos in listPositions: if time_secs - lastUpdate < self.icon_timeout: bpx, bpy = self.relToAbs(rob.x, rob.y, rob.angel, pos[0], pos[1]) ball = self.ball_pool.getBallLabel(rob.color) ball.move(self.screenMidX - int(ball.width() / 2) + self._meter_to_UI(bpx), \ self.screenMidY - int(ball.height() / 2) + self._meter_to_UI(bpy)) rob.currentBallLabel.append(ball) # Message decoding ============================================================================ def setTrajectoryMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) # 5 new lines 6.9. if data.has_key("x"): self.setPathsFor( robotID, [(data.get("x"), data.get("y"), data.get("angle"))]) # Twist vectors: rotation if data.has_key(TrajectoryMsg.label_rotateVel): rob = self.robots.get(robotID) rob.ang_label.setAngles( self.radToDeg(-rob.angel), self.radToDeg(data.get(TrajectoryMsg.label_rotateVel))) # Twist vectors: linear velocity if data.has_key(TrajectoryMsg.label_moveVelX): rob = self.robots.get(robotID) rob.arrow_label.setLinearAngle( self._meter_to_UI(data.get(TrajectoryMsg.label_moveVelX)), self._meter_to_UI(data.get(TrajectoryMsg.label_moveVelY))) rob.arrow_label.move(rob.rob_label.x() + int(rob.rob_label.width() / 2) - rob.arrow_label.width() / 2, \ rob.rob_label.y() + int(rob.rob_label.height() / 2) - rob.arrow_label.height() / 2) rob.arrow_label.setRoboAngle(self.radToDeg(rob.angel)) def setDetectionMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) rob = self.robots.get(robotID) if data.has_key(DetectionMsg.label_ball_info): self.setBallsFor(robotID, [(data.get(DetectionMsg.label_ball_info).get("x"), data.get(DetectionMsg.label_ball_info).get("y"))], \ data.get(DetectionMsg.label_ball_info).get(Name.last_update)) #gamestate translate to obstacle color #if self.own_color == 0: # self.myRobo_color = 3 #elif self.own_color == 1: # self.myRobo_color = 2 if data.has_key(DetectionMsg.label_obstacles): lsOpponents = [] lsUndefined = [] lsMates = [] for ob in data.get(DetectionMsg.label_obstacles): ob_color = ob.get(DetectionMsg.label_obstacle_info).get( DetectionMsg.label_color) if ob_color == rob.team_color: #mitspieler +2 weil die farben lsMates.append([ ob.get(DetectionMsg.label_obstacle_pos).get("x"), ob.get(DetectionMsg.label_obstacle_pos).get("y") ]) elif ob_color == rob.oppo_color: #gegner lsOpponents.append([ ob.get(DetectionMsg.label_obstacle_pos).get("x"), ob.get(DetectionMsg.label_obstacle_pos).get("y") ]) else: #undefined lsUndefined.append([ ob.get(DetectionMsg.label_obstacle_pos).get("x"), ob.get(DetectionMsg.label_obstacle_pos).get("y") ]) """ for ob in data.get(DetectionMsg.label_obstacles): if self.own_color != None: if self.myRobo_color == ob.get("info").get("color"): lsMates.append([ob.get("position").get("x"), ob.get("position").get("y"), self.mate_color]) elif self.myRobo_color != ob.get("info").get("color"): lsOpponents.append([ob.get("position").get("x"), ob.get("position").get("y"), self.op_color]) else : lsUndefined.append([ob.get("position").get("x"), ob.get("position").get("y")]) else : lsUndefined.append([ob.get("position").get("x"), ob.get("position").get("y")]) """ self.setTeammatePosition( robotID, lsMates, data.get(DetectionMsg.label_last_obstacle_update)) self.setObstaclePosition( robotID, lsUndefined, data.get(DetectionMsg.label_last_obstacle_update)) self.setOpponentPosition( robotID, lsOpponents, data.get(DetectionMsg.label_last_obstacle_update)) def setPositionMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) if (data.has_key(PositionMsg.label_pos)): self.setRobotPosition(robotID, data.get(PositionMsg.label_pos).get("x"), data.get(PositionMsg.label_pos).get("y"),\ data.get(PositionMsg.label_orientation).get(PositionMsg.label_yaw)) def setStatusMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) print("add robot: " + robotID) if data.has_key(StatusMsg.labelTeamColor): rob = self.robots.get(robotID) statusColor = data.get(StatusMsg.labelTeamColor) statusColor = ( 1 - statusColor ) + 2 # /gamestate und /obstacles have different constants for colors oppoColor = data.get(StatusMsg.labelTeamColor) + 2 rob.team_color = statusColor rob.oppo_color = oppoColor #print(rob.team_color, rob.oppo_color) """ def setStatusMsg(self, robotID, data): if not self.robots.has_key(robotID): self.addRobot(robotID) # setting cyan & magenta colors for mates/opponents self.own_color = data.get("team_color") if self.own_color != None: if self.own_color == 0: self.mate_color = "cyan" self.op_color = "magenta" elif self.own_color == 1: self.op_color = "cyan" self.mate_color = "magenta" """ # Helper =================================================================== def vec_rotate(self, x, y, angle_rad): xneu = x * math.cos(angle_rad) - y * math.sin(angle_rad) yneu = y * math.cos(angle_rad) + x * math.sin(angle_rad) return [xneu, yneu] def relToAbs(self, fromx, fromy, fromAng, relx, rely): rx, ry = self.vec_rotate(relx, rely, fromAng) #- self.degToRadians(90)) return (fromx + rx, fromy + ry) def degToRadians(self, deg): return deg / 57.29578 def radToDeg(self, rads): return rads * 57.29578