Beispiel #1
0
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))
Beispiel #2
0
    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
Beispiel #3
0
 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
Beispiel #4
0
 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)
Beispiel #6
0
    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
Beispiel #8
0
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()])