class LocalMapFrame(wx.Frame): def __init__(self, parent, ms, map_name, id=wx.ID_ANY, pos=wx.DefaultPosition, size=(512,512), style=wx.DEFAULT_FRAME_STYLE): title = map_name self.map_name = map_name wx.Frame.__init__(self, parent, id, title, pos, size, style) wx.GetApp().local_map_windows[map_name] = self self.ms = ms self.ms = MapService() self.wnd = LocalMapWindow(self, self.ms, map_name) self.wnd.SetSize(self.GetSize()) self.status_bar = wx.StatusBar(self) self.SetStatusBar(self.status_bar) self.SetSize((size[0], size[1] + self.status_bar.GetSize()[1])) self.wnd.Bind(wx.EVT_MOUSE_EVENTS, self.OnMouse) def OnMouse(self, event): #~ if not event.LeftDown() and not event.RightDown(): #~ return #~ log.debug("OnMouse(%s, %s)" % (event.GetX(), event.GetY())) if event.ButtonDClick(): log.debug("Close!") wx.GetApp().local_map_windows.pop(self.map_name) self.Close() hit = self.wnd.HitTest(event.GetX(), event.GetY()) if hit: hit = hit.door self.status_bar.SetStatusText(str(hit), 0) else: self.status_bar.SetStatusText("", 0) if hit and event.LeftDown(): log.debug("hit: %s" % hit) other_side = self.ms.other_side(hit) log.debug("other_side: %s" % other_side) if other_side.map_name not in wx.GetApp().local_map_windows: log.debug("other_side.map_name: %s" % other_side.map_name) win = LocalMapFrame(None, self.ms, other_side.map_name) win.Show() if not hit and event.LeftDown(): map_size = self.ms.map_size(self.map_name) zoom = map_size[0] / 512. loc = (int(event.GetX() * zoom), map_size[1] - int(event.GetY() * zoom)) log.debug("OnMouse(%s, %s) -> (%s, %s)" % (event.GetX(), event.GetY(), loc[0], loc[1])) wx.GetApp().SetNavTo(Location(self.map_name, loc))
def Draw(self): log.debug("Draw()") dc = wx.BufferedPaintDC(self, self._Buffer) dc.Clear() if not self.image: return png_dc = wx.MemoryDC() png_dc.SelectObject(self.image) dc.Blit(0, 0, self.image.GetWidth(), self.image.GetHeight(), png_dc, 0, 0) dc.SetPen(wx.Pen('black')) for item in self.sprites: item.Draw(dc) if self.current_map: dc.SetPen(wx.Pen('yellow')) self.current_map.Draw(dc) ms = MapService() ds = DistanceService(ms) if wx.GetApp().GetNavTo(): dc.SetPen(wx.Pen('red')) DrawElLocation(dc, wx.GetApp().GetNavTo()) dc.SetPen(wx.Pen('yellow')) for item in ms.doors(wx.GetApp().GetNavTo().map_name): DrawElLocation(dc, item) dc.SetPen(wx.Pen('green')) DrawElLocation(dc, ds.nearest_door(wx.GetApp().GetNavTo())) if wx.GetApp().GetNavFrom(): dc.SetPen(wx.Pen('blue')) DrawElLocation(dc, wx.GetApp().GetNavFrom()) dc.SetPen(wx.Pen('yellow')) for item in ms.doors(wx.GetApp().GetNavFrom().map_name): DrawElLocation(dc, item) dc.SetPen(wx.Pen('green')) DrawElLocation(dc, ds.nearest_door(wx.GetApp().GetNavFrom())) if wx.GetApp().GetNavTo() and wx.GetApp().GetNavFrom(): route = wx.GetApp().GetRoute() #~ for item in route: #~ log.debug(item) dc.SetPen(wx.Pen('blue')) last_pos = el_to_dc(route[0].payload) for item in route[1:]: pos = el_to_dc(item.payload) dc.DrawLine(last_pos[0], last_pos[1], pos[0], pos[1]) last_pos = pos
def __init__(self): wx.App.__init__(self) self.ms = MapService() self.ds = DistanceService(self.ms) self.local_map_windows = {} self.__nav_from__ = None self.__nav_to__ = None self.__route__ = None
def thrift_client(self): socket = TSocket.TSocket(self.ip, self.port) transport = TTransport.TBufferedTransport(socket) protocol = TBinaryProtocol.TBinaryProtocolAccelerated(transport) client = MapService.Client(protocol) transport.open() yield client transport.close()
def __init__(self, pubs, galileo_status, galileo_status_lock, host='', user_socket_port=20001, buf_size=1024): super(MonitorServer, self).__init__() self.host = host self.usersocket_port = user_socket_port self.buf_size = buf_size self._stop = threading.Event() self.user_server_socket = socket(AF_INET, SOCK_DGRAM) self.user_server_socket.bind((self.host, self.usersocket_port)) self.parser = ReqParser() self.speed_cmd = Twist() self.global_move_pub = pubs["GLOBAL_MOVE_PUB"] self.cmd_vel_pub = pubs["CMD_VEL_PUB"] self.map_save_pub = pubs["MAPSAVE_PUB"] self.elevator_pub = pubs["ELEVATOR_PUB"] self.tilt_pub = pubs["TILT_PUB"] self.charge_pub = pubs["CHARGE_PUB"] self.charge_pose_pub = pubs["CHARGE_POSE_PUB"] self.galileo_status = galileo_status self.galileo_status_lock = galileo_status_lock self.map_thread = MapService(self.galileo_status, self.galileo_status_lock) self.nav_thread = NavigationService(self.galileo_status, self.galileo_status_lock) self.last_nav_time = rospy.Time(0) self.user_socket_remote = None self.nav_task = None self.nav_guide_task = None self.busy_flag = False rospy.loginfo("service started") def get_galileo_cmds(cmds): self.parse_data([map(lambda x: ord(x), list(cmds.data))]) self.cmd_sub = rospy.Subscriber('/galileo/cmds', GalileoNativeCmds, get_galileo_cmds)
def __init__(self, parent, ms, map_name, id=wx.ID_ANY, pos=wx.DefaultPosition, size=(512,512), style=wx.DEFAULT_FRAME_STYLE): title = map_name self.map_name = map_name wx.Frame.__init__(self, parent, id, title, pos, size, style) wx.GetApp().local_map_windows[map_name] = self self.ms = ms self.ms = MapService() self.wnd = LocalMapWindow(self, self.ms, map_name) self.wnd.SetSize(self.GetSize()) self.status_bar = wx.StatusBar(self) self.SetStatusBar(self.status_bar) self.SetSize((size[0], size[1] + self.status_bar.GetSize()[1])) self.wnd.Bind(wx.EVT_MOUSE_EVENTS, self.OnMouse)
class MonitorServer(threading.Thread): # 接收udp命令的socket def __init__(self, pubs, galileo_status, galileo_status_lock, host='', user_socket_port=20001, buf_size=1024): super(MonitorServer, self).__init__() self.host = host self.usersocket_port = user_socket_port self.buf_size = buf_size self._stop = threading.Event() self.user_server_socket = socket(AF_INET, SOCK_DGRAM) self.user_server_socket.bind((self.host, self.usersocket_port)) self.parser = ReqParser() self.speed_cmd = Twist() self.global_move_pub = pubs["GLOBAL_MOVE_PUB"] self.cmd_vel_pub = pubs["CMD_VEL_PUB"] self.map_save_pub = pubs["MAPSAVE_PUB"] self.elevator_pub = pubs["ELEVATOR_PUB"] self.tilt_pub = pubs["TILT_PUB"] self.charge_pub = pubs["CHARGE_PUB"] self.charge_pose_pub = pubs["CHARGE_POSE_PUB"] self.galileo_status = galileo_status self.galileo_status_lock = galileo_status_lock self.map_thread = MapService(self.galileo_status, self.galileo_status_lock) self.nav_thread = NavigationService(self.galileo_status, self.galileo_status_lock) self.last_nav_time = rospy.Time(0) self.user_socket_remote = None self.nav_task = None self.nav_guide_task = None self.busy_flag = False rospy.loginfo("service started") def get_galileo_cmds(cmds): self.parse_data([map(lambda x: ord(x), list(cmds.data))]) self.cmd_sub = rospy.Subscriber('/galileo/cmds', GalileoNativeCmds, get_galileo_cmds) def stop(self): self.cmd_sub.unregister() if self.user_server_socket != None: self.user_server_socket.close() if not self.map_thread.stopped(): self.map_thread.stop() self._stop.set() def stopped(self): return self._stop.isSet() def run(self): self.user_server_socket.settimeout(2) # 设置udp 2秒超时 等待 while not self.stopped() and not rospy.is_shutdown(): try: data, self.user_socket_remote = self.user_server_socket.recvfrom( self.buf_size) except timeout: continue if not data: break dataList = [] for c in data: dataList.append(ord(c)) self.parse_data(self.parser.unpack_req(dataList)) # 处理命令数据 self.stop() def parse_data(self, cmds): # 等待退出busy状态,最长等待3分钟 wait_busy_count = 3 * 60 * 100 while self.busy_flag and wait_busy_count > 0: time.sleep(0.01) wait_busy_count -= 1 self.busy_flag = True time_now = rospy.Time.now() for count in range(0, len(cmds)): if len(cmds[count]) > 0: self.CONTROL_FLAG = True global_move_flag = Bool() global_move_flag.data = True self.global_move_pub.publish(global_move_flag) if len(cmds[count]) == 2: global_move_flag = Bool() global_move_flag.data = True temp_scale = 1.0 if abs(self.speed_cmd.linear.x) < 1.0: temp_scale = 1.0 else: temp_scale = abs(self.speed_cmd.linear.x) # 判断是否为关机命令 if cmds[count][0] == 0xaa and cmds[count][1] == 0x44: rospy.loginfo("system poweroff") commands.getstatusoutput( 'sudo shutdown -h now') if cmds[count][0] == ord('f'): rospy.loginfo("forward") self.global_move_pub.publish(global_move_flag) self.speed_cmd.linear.x = MAX_VEL * cmds[count][1] / 100.0 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('b'): rospy.loginfo("back") self.global_move_pub.publish(global_move_flag) self.speed_cmd.linear.x = -MAX_VEL * cmds[count][1] / 100.0 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('c'): rospy.loginfo("circleleft") self.global_move_pub.publish(global_move_flag) if cmds[count][1] > 1: self.speed_cmd.angular.z = max( 0.4, MAX_THETA * cmds[count][1] / 100.0 / temp_scale) else: self.speed_cmd.angular.z = MAX_THETA * \ cmds[count][1] / 100.0 / temp_scale #self.SPEED_CMD.angular.z = MAX_THETA * cmds[count][1]/100.0/2.8 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('d'): rospy.loginfo("circleright") self.global_move_pub.publish(global_move_flag) if cmds[count][1] > 1: self.speed_cmd.angular.z = min( -0.4, -MAX_THETA * cmds[count][1] / 100.0 / temp_scale) else: self.speed_cmd.angular.z = -MAX_THETA * \ cmds[count][1] / 100.0 / temp_scale #self.SPEED_CMD.angular.z = -MAX_THETA * cmds[count][1]/100.0/2.8 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('s'): rospy.loginfo("stop") self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('V'): if cmds[count][1] == 0: rospy.loginfo("开启视觉") rospy.loginfo("关闭导航") if self.nav_task is not None: self.nav_task.shutdown() self.nav_task = None tilt_degree = Int16() tilt_degree.data = 0 self.tilt_pub.publish(tilt_degree) if not self.nav_thread.stopped(): self.nav_thread.stop() self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) os.system("pkill -f 'roslaunch nav_test tank_blank_map0.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map1.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map2.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map3.launch'") if self.map_thread.stopped(): rospy.loginfo("开启视觉2") self.map_thread.update = False self.map_thread.start() elif cmds[count][1] == 1: rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") elif cmds[count][1] == 2: rospy.loginfo("保存地图") mapSaveFlag = Bool() mapSaveFlag.data = True self.map_save_pub.publish(mapSaveFlag) if self.map_thread.scale_orb_thread != None: self.map_thread.scale_orb_thread.save_scale() elif cmds[count][1] == 3: rospy.loginfo("更新地图") if self.map_thread.stopped(): rospy.loginfo("开启视觉2") self.map_thread.update = True self.map_thread.start() elif cmds[count][0] == ord('h'): elePose = UInt32() elePose.data = cmds[count][1] self.elevator_pub.publish(elePose) elif cmds[count][0] == ord('m'): time1_diff = time_now - self.last_nav_time if cmds[count][1] == 0: if time1_diff.to_sec() < 30: continue rospy.loginfo("开启视觉,不巡检") rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") self.last_nav_time = time1_diff tilt_degree = Int16() tilt_degree.data = -19 self.tilt_pub.publish(tilt_degree) if self.nav_thread.stopped(): self.nav_thread.setspeed(0) self.nav_thread.start() if self.nav_task is not None: self.nav_task.shutdown() self.nav_task = NavigationTask() if cmds[count][1] == 4: rospy.loginfo("关闭自主巡检") rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") if self.nav_task is not None: self.nav_task.shutdown() self.nav_task = None tilt_degree = Int16() tilt_degree.data = 0 self.tilt_pub.publish(tilt_degree) if not self.nav_thread.stopped(): self.nav_thread.stop() if self.nav_task is not None: self.nav_task.shutdown() self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) os.system("pkill -f 'roslaunch nav_test tank_blank_map0.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map1.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map2.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map3.launch'") if cmds[count][1] == 5: rospy.loginfo("开启自动巡检") rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") tilt_degree = Int16() tilt_degree.data = -19 self.tilt_pub.publish(tilt_degree) if self.nav_thread.stopped(): rospy.logwarn("巡检状态未启动") self.busy_flag = False return if self.nav_task.loop_running_flag: rospy.logwarn("已经开始巡检") self.busy_flag = False return self.nav_task.start_loop() if cmds[count][1] == 6: rospy.loginfo("停止自动巡检") if self.nav_task is not None: self.nav_task.stop_loop() elif cmds[count][0] == ord('g'): if self.nav_task is None: self.busy_flag = False return self.nav_task.set_goal(cmds[count][1]) elif cmds[count][0] == ord('i'): if self.nav_task is None: self.busy_flag = False return if cmds[count][1] == 0: self.nav_task.pause() if cmds[count][1] == 1: self.nav_task.resume() if cmds[count][1] == 2: self.nav_task.cancel_goal() elif cmds[count][0] == ord('j'): if cmds[count][1] == 0: # start charge if self.nav_task is not None: charge_msg = Bool() charge_msg.data = True self.charge_pub.publish(charge_msg) if cmds[count][1] == 1: # stop charge charge_msg = Bool() charge_msg.data = False self.charge_pub.publish(charge_msg) if cmds[count][1] == 2: # save charge position save_msg = Bool() save_msg.data = True self.charge_pose_pub.publish(save_msg) if len(cmds[count]) > 2: # insert new goal if cmds[count][0] == ord("g") and cmds[count][1] == ord("i"): pos_x = struct.unpack("f", bytearray(cmds[count][2:6]))[0] pos_y = struct.unpack("f", bytearray(cmds[count][6:10]))[0] if self.nav_task is not None: rospy.logwarn("插入目标点: " + str(pos_x) + " " + str(pos_y)) self.nav_task.insert_goal(pos_x, pos_y, 0) # reset goals if cmds[count][0] == ord("g") and cmds[count][1] == ord("r"): if self.nav_task is not None: self.nav_task.reset_goals() # set loop and sleep time if cmds[count][0] == ord("m") and cmds[count][1] == 5: if self.nav_thread.stopped(): rospy.logwarn("巡检状态未启动") self.busy_flag = False return if self.nav_task is not None: if not self.nav_task.loop_running_flag: tilt_degree = Int16() tilt_degree.data = -19 self.tilt_pub.publish(tilt_degree) self.nav_task.sleep_time = cmds[count][2] self.nav_task.start_loop() else: self.nav_task.sleep_time = cmds[count][2] self.busy_flag = False def sendto(self, data): try: if self.get_connection_status(): self.user_server_socket.sendto( bytes(data), self.user_socket_remote) except: rospy.logwarn("remote disconnect !") def get_connection_status(self): if self.user_server_socket != None and self.user_socket_remote != None: return True else: return False
class EhApp(wx.App): def __init__(self): wx.App.__init__(self) self.ms = MapService() self.ds = DistanceService(self.ms) self.local_map_windows = {} self.__nav_from__ = None self.__nav_to__ = None self.__route__ = None def GetLocalMapWindow(self, map_name): if map_name in self.local_map_windows: return self.local_map_windows[map_name] win = LocalMapFrame(None, self.ms, map_name) win.Show() self.local_map_windows[map_name] = win return win def SetNavFrom(self, loc): self.__nav_from__ = loc self.__nav_from_to__changed() def GetNavFrom(self): return self.__nav_from__ def SetNavTo(self, loc): self.__nav_to__ = loc self.__nav_from_to__changed() def __nav_from_to__changed(self): if self.__nav_from__ and self.__nav_to__: self.__calc_route__() for item in self.local_map_windows.values(): item.wnd.Draw() def GetNavTo(self): return self.__nav_to__ def GetRoute(self): return self.__route__ def __calc_route__(self): node_dict = {} map_dict = {} for item in self.ms.all_doors(): node = Node(item) node_dict[item] = node if not item.map_name in map_dict: map_dict[item.map_name] = [] map_dict[item.map_name].append(node) from_node = Node(wx.GetApp().GetNavFrom()) map_dict[wx.GetApp().GetNavFrom().map_name].append(from_node) to_node = Node(wx.GetApp().GetNavTo()) map_dict[wx.GetApp().GetNavTo().map_name].append(to_node) #beam beam_node = Node(self.ms.beam_location()) log.debug("beam_node: %s" % beam_node) from_node.edges.append((1, beam_node)) map_dict[beam_node.payload.map_name].append(beam_node) for map_name, map_nodes in map_dict.items(): for index, item_a in enumerate(map_nodes): for item_b in map_nodes[index + 1:]: cost = self.ds.in_map_distance(item_a.payload, item_b.payload) add_edge(cost, item_a, item_b) for item, node in node_dict.items(): other_item = self.ms.other_side(item) cost = self.ds.cost(item, other_item) cost = 1 node.edges.append((cost, node_dict[other_item])) node_dict[wx.GetApp().GetNavFrom()] = from_node node_dict[wx.GetApp().GetNavTo()] = to_node node_dict[self.ms.beam_location()] = beam_node nodes = node_dict.values() for item in nodes: log.debug(item) for item_b in item.edges: log.debug(" %s" % str(item_b)) from_node.cost = 0 solve(nodes) for item in nodes: log.debug(item) log.debug("ROUTE:") self.__route__ = get_route(nodes, node_dict[wx.GetApp().GetNavTo()])