def P4(graph: nx.Graph, central_hyperedge, image: Image): copy = graph.copy() try: run_P4(graph, central_hyperedge, image) except Exception: graph.clear() graph.add_nodes_from(copy) graph.add_edges_from(copy.edges) raise
def P4(graph: nx.Graph, central_hyperedge, image: Image): copy = graph.copy() try: run_P4(graph, central_hyperedge, image) except Exception: graph.clear() x = [(k, v) for k, v in copy.nodes(data=True)] graph.add_nodes_from(x) graph.add_edges_from(copy.edges) raise
def clear(self): Graph.clear(self) self.fh.write("Clear graph\n")
class WaypointServer: def __init__(self): self.server = InteractiveMarkerServer("waypoint_server") self.waypoint_graph = Graph() self.next_waypoint_id = 0 self.next_edge_id = 0 self.state = STATE_REGULAR self.connect_first_marker = "" self.edge_line_publisher = rospy.Publisher("~edges", MarkerArray, queue_size=10) self.marker_frame = rospy.get_param("~marker_frame", "map") self.uuid_name_map = {} self.removeService = rospy.Service('~remove_edge', RemoveEdge, self.remove_edge_service_call) self.loadService = rospy.Service('~load_waypoints', LoadWaypoints, self.load_waypoints_service) self.saveService = rospy.Service('~save_waypoints', SaveWaypoints, self.save_waypoints_service) self.getWaypointGraphService = rospy.Service( '~get_waypoint_graph', GetWaypointGraph, self.get_waypoint_graph_service_call) rospy.Subscriber("/clicked_point", PointStamped, self.insert_marker_callback) rospy.on_shutdown(self.clear_all_markers) rospy.logwarn( "The waypoint server is waiting for RViz run and to subscribe to {0}." .format(rospy.resolve_name("~edges"))) while self.edge_line_publisher.get_num_connections() == 0: rospy.sleep(1.0) self.clear_all_markers() load_file = rospy.get_param("~waypoint_file", "") # yaml file with waypoints to load if len(load_file) != 0: rospy.loginfo( "Waypoint_Server is loading initial waypoint file {0}.".format( load_file)) server.load_waypoints_from_file(load_file) def insert_marker_callback(self, pos): rospy.logdebug( "Inserting new waypoint at position ({0},{1},{2}).".format( pos.point.x, pos.point.y, pos.point.z)) self.insert_marker(pos.point) def clear_all_markers(self): edges = MarkerArray() marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = self.marker_frame marker.ns = "waypoint_edges" marker.id = 0 marker.action = Marker.DELETEALL edges.markers.append(marker) self.edge_line_publisher.publish(edges) self.waypoint_graph.clear() def _make_marker(self, msg): marker = Marker() marker.type = Marker.SPHERE marker.scale.x = msg.scale * 0.45 marker.scale.y = msg.scale * 0.45 marker.scale.z = msg.scale * 0.45 marker.color.r = 0 marker.color.g = 1 marker.color.b = 0 marker.color.a = 1.0 return marker def _make_edge(self, scale, begin, end): marker = Marker() marker.header.frame_id = self.marker_frame marker.header.stamp = rospy.Time.now() marker.ns = "waypoint_edges" marker.id = self.next_edge_id self.next_edge_id += 1 marker.type = Marker.LINE_LIST marker.action = Marker.ADD marker.scale.x = scale * 0.45 marker.scale.y = scale * 0.45 marker.scale.z = scale * 0.45 marker.color.r = 0 marker.color.g = 0 marker.color.b = 1 marker.color.a = 1.0 marker.points.append(begin) marker.points.append(end) return marker def _remove_marker(self, name): self._clear_marker(name) self.waypoint_graph.remove_node(name) del self.uuid_name_map[name] self.server.erase(name) self.server.applyChanges() def _clear_marker(self, name): # remove all edges to a waypoint edges = MarkerArray() to_remove = [] for u, v, marker in self.waypoint_graph.edges(name, data='marker'): marker.action = Marker.DELETE to_remove.append((u, v, marker)) # update knowledge database to remove all edges for u, v, marker in to_remove: edges.markers.append(marker) self.waypoint_graph.remove_edge(u, v) self.edge_line_publisher.publish(edges) # publish deletion def _connect_markers(self, u, v, cost=0.0): if self.waypoint_graph.has_edge(u, v): # remove edge edges = MarkerArray() marker = self.waypoint_graph.get_edge_data(u, v)["marker"] marker.action = Marker.DELETE edges.markers.append(marker) self.waypoint_graph.remove_edge(u, v) self.edge_line_publisher.publish(edges) # publish deletion else: name_u = self.uuid_name_map[u] name_v = self.uuid_name_map[v] u_pos = self.server.get(u).pose.position v_pos = self.server.get(v).pose.position # insert edge marker = self._make_edge(0.2, u_pos, v_pos) marker.text = str(cost) if cost is not 0 else "" # insert edge into graph self.waypoint_graph.add_edge(u, v, u=u, v=v, cost=cost, marker=marker) self.update_edges() def update_edges(self): edges = MarkerArray() for u, v, marker in self.waypoint_graph.edges(data='marker'): edges.markers.append(marker) self.edge_line_publisher.publish(edges) def process_feedback(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: handle = feedback.menu_entry_id if handle == MENU_CONNECT: self.state = STATE_CONNECT self.connect_first_marker = feedback.marker_name elif handle == MENU_CLEAR: self._clear_marker(feedback.marker_name) elif handle == MENU_REMOVE: self._remove_marker(feedback.marker_name) elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: if self.state == STATE_CONNECT: self.state = STATE_NONE self._connect_markers(self.connect_first_marker, feedback.marker_name) elif self.state == STATE_NONE: pass # ignore else: pos = feedback.pose.position rospy.logdebug( "Updateing pose of marker {3} to ({0},{1},{2})".format( pos.x, pos.y, pos.z, feedback.marker_name)) # update database # push to scene database pstamped = PoseStamped() pstamped.header.frame_id = self.marker_frame pstamped.pose = feedback.pose elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: if self.state == STATE_NONE: self.state = STATE_REGULAR self.server.applyChanges() def move_feedback(self, feedback): pose = feedback.pose self.server.setPose(feedback.marker_name, pose) # correct all edges for u, v, data in self.waypoint_graph.edges([feedback.marker_name], data=True): if feedback.marker_name == data["u"]: data["marker"].points[0] = pose.position else: data["marker"].points[1] = pose.position self.update_edges() self.server.applyChanges() def insert_marker(self, position, name=None, uuid=None, frame_id=None): if frame_id is None: frame_id = self.marker_frame if uuid is None: uuid = unique_id.fromRandom() if name is None: name = "wp{0}".format(self.next_waypoint_id) self.next_waypoint_id += 1 self.uuid_name_map[str(uuid)] = name # insert waypoint int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.pose.position = position int_marker.pose.orientation.w = 1 int_marker.scale = 0.5 int_marker.name = str(uuid) int_marker.description = name # make markers moveable in the plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE # interactive menu for each marker menu_handler = MenuHandler() menu_handler.insert("Connect/Disconnect", callback=self.process_feedback) menu_handler.insert("Clear", callback=self.process_feedback) menu_handler.insert("Remove", callback=self.process_feedback) # make a box which also moves in the plane control.markers.append(self._make_marker(int_marker)) control.always_visible = True int_marker.controls.append(control) # we want to use our special callback function self.server.insert(int_marker, self.process_feedback) menu_handler.apply(self.server, int_marker.name) # set different callback for POSE_UPDATE feedback pose_update = InteractiveMarkerFeedback.POSE_UPDATE self.server.setCallback(int_marker.name, self.move_feedback, pose_update) self.server.applyChanges() # insert into graph self.waypoint_graph.add_node(str(uuid)) def save_waypoints_service(self, request): filename = request.file_name self.save_waypoints_to_file(filename) rospy.loginfo("Saved waypoints to {0}".format(filename)) return SaveWaypointsResponse() def load_waypoints_service(self, request): filename = request.file_name self.load_waypoints_from_file(filename) rospy.loginfo("Loaded waypoints from {0}".format(filename)) return LoadWaypointsResponse() def save_waypoints_to_file(self, filename): data = {"waypoints": {}, "edges": []} for uuid in self.waypoint_graph.nodes(): name = self.uuid_name_map[uuid] pos = self.server.get(uuid).pose.position data["waypoints"].update( {uuid: { "name": name, "x": pos.x, "y": pos.y, "z": pos.z }}) for u, v, cost in self.waypoint_graph.edges(data='cost'): data["edges"].append({'u': u, 'v': v, 'cost': cost}) with open(filename, 'w') as f: yaml.dump(data, f, default_flow_style=False) def load_waypoints_from_file(self, filename): self.clear_all_markers() self.server.clear() with open(filename, 'r') as f: data = yaml.load(f) for uuid, wp in data["waypoints"].items(): point = Point(wp["x"], wp["y"], wp["z"]) self.insert_marker(position=point, uuid=uuid, name=wp['name']) for edge in data["edges"]: self._connect_markers(edge['u'], edge['v'], edge['cost']) def remove_edge_service_call(self, request): if self.waypoint_graph.has_edge(request.u, request.v): self._connect_markers(request.u, request.v) return RemoveConnectionResponse() def get_waypoint_graph_service_call(self, request): ret = GetWaypointGraphResponse() for r in request.waypoints: if not self.waypoint_graph.has_node(str(r)): raise rospy.ServiceException("invalid waypoint {:}".format(r)) if len(request.waypoints) == 0: graph = self.waypoint_graph else: graph = self.waypoint_graph.subgraph(request.waypoints) for node in graph.nodes(): pose = self.server.get(node).pose ret.names.append(node) ret.positions.append(pose) for u, v in graph.edges(): e = Edge() e.source = u e.target = v ret.edges.append(e) return ret