1.优化手动控制模块(绕过板子MPC程序直接控制)

2.优化切换模式通信协议

ControllWidget.py

@@ -212,9 +212,9 @@ class ControlFrame(QFrame):
     def MainAgvchangecb(self):
         if self.robot_.IsMainModeStatu() == False:
             leng = float(self.WheelBaseEdit.text())
-            self.threadPool_.submit(self.robot_.SwitchMoveMode, 2, leng)
+            self.threadPool_.submit(self.robot_.SwitchMoveMode, 1, leng)
         else:
-            self.threadPool_.submit(self.robot_.SwitchMoveMode, 1, 0)
+            self.threadPool_.submit(self.robot_.SwitchMoveMode, 0, 0)
 
     def endStreetQCChanged(self):
         id1 = self.begQc.currentText()
@@ -239,13 +239,13 @@ class ControlFrame(QFrame):
                                         self.begId_, self.targetId_)
             else:
                 print("agv not in main statu")
-        else :
+        else:
             autoDirect=False
             if self.btnAutoDirect.checkState()==Qt.Checked:
                 print("自由方向")
                 autoDirect=True
             self.threadPool_.submit(self.robot_.Navigatting,
-                                    self.begId_, self.targetId_,autoDirect)
+                                    self.begId_, self.targetId_,autoDirect,float(self.WheelBaseEdit.text()))
 
     def btnCancelClick(self):
         self.threadPool_.submit(self.robot_.CancelNavTask)

ManualOperationWidget.py

@@ -1,5 +1,3 @@
-import enum
-import time
 from concurrent.futures import ThreadPoolExecutor
 
 from PyQt5.QtCore import QTimer
@@ -8,16 +6,7 @@ from PyQt5.QtWidgets import QFrame
 from UI.manual_operation import Ui_manual_operation_widget
 
 
-class ManualOperationType(enum.Enum):
-    eReady = 0x0000
-
-    eCounterclockwiseRotate = 0x0001
-    eClockwiseRotate = 0x0002
-
-    eForwardMove = 0x0010,
-    eBackwardMove = 0x0020,
-    eLeftMove = 0x0040,
-    eRightMove = 0x0080,
+from custom_define import ManualOperationType, RobotName
 
 
 class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
@@ -53,13 +42,20 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
         self.Right_Move_btn.pressed.connect(self.Right_Move)
         self.Right_Move_btn.released.connect(self.Cancel)
 
+        # 加减速阶梯
+        self.step_acc: int = 0
+
         # 启动界面刷新定时器
         self.timer = QTimer()
         self.timer.timeout.connect(self.DownCmd)
-        self.timer.start(200)
+        self.timer.start(50)
 
     def SelectRobot(self):
-        self.selected_robot_name = self.select_box.currentText()
+        if self.robot_dict[RobotName.strAGVMain].IsMainModeStatu():
+            self.selected_robot_name = RobotName.strAGVMain
+        else:
+            self.selected_robot_name = self.select_box.currentText()
+
     def Counterclockwise_Rotate(self):
         self.current_operation = ManualOperationType.eCounterclockwiseRotate
 
@@ -84,13 +80,17 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
 
     def DownCmd(self):
         if self.current_operation != ManualOperationType.eReady:
-            self.threadPool_.submit(self.robot_dict[self.selected_robot_name].ManualTask, self.current_operation)
+            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, self.step_acc)
+            self.step_acc += 1
+            # self.threadPool_.submit(self.robot_dict[self.selected_robot_name].ManualTask, self.current_operation)
         elif self.operation_count[ManualOperationType.eReady] != 0:
             # self.threadPool_.shutdown(False)
             self.threadPool_.submit(self.robot_dict[self.selected_robot_name].CancelNavTask)
             self.current_operation = ManualOperationType.eReady
             self.operation_count[ManualOperationType.eReady] = 0
+            self.step_acc = 0
         else:
             self.current_operation = ManualOperationType.eReady
             self.operation_count[ManualOperationType.eReady] = 0
+            self.step_acc = 0
 

RobotData.py

@@ -5,8 +5,10 @@ import message_pb2 as message
 import grpc
 import message_pb2_grpc as msg_rpc
 from ManualOperationWidget import ManualOperationType
+from custom_define import RobotName, ActType
 from mqtt_async import MqttAsync
 import google.protobuf.json_format as jtf
+import json
 from dijkstra.Map import DijikstraMap, SpaceNode, StreetNode
 import uuid
 import random
@@ -15,16 +17,6 @@ import Count
 from sigmoid import sigmoid
 
 
-class ActType(enum.Enum):
-    eReady = 0
-    eRotation = 1
-    eHorizon = 2
-    eVertical = 3
-    eMPC = 4
-    eClampOpen = 5
-    eClampClose = 6
-
-
 class TimeStatu:
     def __init__(self, statu=None, timeout=3):
         self.statu = statu
@@ -40,6 +32,7 @@ class Robot(MqttAsync):
 
     def __init__(self, rpc_ipport, name=""):
         MqttAsync.__init__(self)
+        self.test_speed = 0
         self.timedRobotStatu_ = TimeStatu(message.RobotStatu, 0)
         self.timedNavStatu_ = TimeStatu(message.NavStatu, 0)
         self.dataTopic_ = {}
@@ -47,7 +40,10 @@ class Robot(MqttAsync):
         self.messageArrivedSignal = None
         self.currentNavData_ = None
         self.navCmdTopic_ = None
-        self.speedCmdTopic = "monitor_child/speedcmd"
+        if(name == RobotName.strAGVMain):
+            self.speedCmdTopic = "monitor_child/speedcmd"
+        else:
+            self.speedCmdTopic = "monitor_child/speedcmd"
         self.currentNavPathNodes_ = None
         self.currentNavPath_ = None
 
@@ -79,7 +75,7 @@ class Robot(MqttAsync):
     def IsMainModeStatu(self):
         if self.IsMainAgv():
             if self.timedNavStatu_.timeout() == False:
-                if self.timedNavStatu_.statu.move_mode == 2:
+                if self.timedNavStatu_.statu.move_mode == 1:
                     return True
         return False
 
@@ -183,11 +179,11 @@ class Robot(MqttAsync):
     阻塞直到导航完成
     '''
 
-    def Navigatting(self, begId, targetId, autoDirect, task_type="None"):
+    def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type="None"):
         print("nav")
         self.GeneratePath(begId, targetId, task_type=task_type)
         # self.ExecPathNodes(autoDirect)
-        self.ExecNavtask(autoDirect)
+        self.ExecNavtask(autoDirect, wheelbase)
 
         if self.IsMainModeStatu():
             Count.TestCount().loadCountAdd()
@@ -249,7 +245,7 @@ class Robot(MqttAsync):
         protoNode.id = node.id_
         return protoNode
 
-    def ExecNavtask(self, autoDirect):
+    def ExecNavtask(self, autoDirect, wheelbase=0):
         cmd = message.NavCmd()
         key = str(uuid.uuid4())
         cmd.key = key
@@ -264,7 +260,7 @@ class Robot(MqttAsync):
                 protoStreet = self.generateProtoNode(street)
                 act = message.NewAction()
                 act.type = 1
-                act.wheelbase = 2.78
+                act.wheelbase = wheelbase
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
 
@@ -275,7 +271,7 @@ class Robot(MqttAsync):
                 protoStreet = self.generateProtoNode(street)
                 act = message.NewAction()
                 act.type = 2
-                act.wheelbase = 2.78
+                act.wheelbase = wheelbase
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
 
@@ -294,7 +290,6 @@ class Robot(MqttAsync):
 
         channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
-
         response = stub.Start(cmd)
         print("client received: ", response)
         if not response.ret == 0:
@@ -368,10 +363,10 @@ class Robot(MqttAsync):
         y_offset = 1.2
         dy = 0
         # if self.IsMainAgv() or self.name_ == "AgvMain":
-        if self.name_ == "AgvMain":
+        if self.name_ == RobotName.strAGVMain:
             dy = 1
         # if not self.IsMainAgv() or self == "AGV2":
-        if self.name_ == "AGV2":
+        if self.name_ == RobotName.strAGV2:
             dy = -1
         print("dy:", dy)
         for node in adjusted_path:
@@ -445,7 +440,7 @@ class Robot(MqttAsync):
         action = message.NewAction()
         action.type = 8
         action.changedMode = mode
-        if mode == 2:
+        if mode == 1:
             action.wheelbase = wheelbase
         cmd.newActions.add().CopyFrom(action)
         channel = grpc.insecure_channel(self.rpc_ipport_)
@@ -530,43 +525,57 @@ class Robot(MqttAsync):
         print(" Cancel task completed!!!")
         return True
 
-    def ManualTask(self, current_operation):
+    def ManualTask(self, current_operation, step_acc):
+        if self.IsNavigating():
+            print("AGV is navigating!!!")
+            return
+
         cmd = message.ToAgvCmd()
         self.heat_ += 1
         self.heat_ %= 255
         cmd.H = self.heat_
         if self.IsMainModeStatu():
-            cmd.M = 2
-        else:
             cmd.M = 1
+        else:
+            cmd.M = 0
 
         if current_operation == ManualOperationType.eCounterclockwiseRotate:
             cmd.T = 1
             cmd.V = 0
-            cmd.W = 2.0/180*math.pi
+            cmd.W = 4.0 / 180 * math.pi * sigmoid(step_acc, 0, 1)
         if current_operation == ManualOperationType.eClockwiseRotate:
             cmd.T = 1
             cmd.V = 0
-            cmd.W = -2.0 / 180 * math.pi
+            cmd.W = -4.0 / 180 * math.pi * sigmoid(step_acc, 0, 1)
         if current_operation == ManualOperationType.eForwardMove:
             cmd.T = 3
-            cmd.V = 0.5
+            cmd.V = 0.6 * sigmoid(step_acc, 0, 1)
             cmd.W = 0
         if current_operation == ManualOperationType.eBackwardMove:
             cmd.T = 3
-            cmd.V = -0.5
+            cmd.V = -0.6 * sigmoid(step_acc, 0, 1)
             cmd.W = 0
         if current_operation == ManualOperationType.eLeftMove:
             cmd.T = 2
-            cmd.V = 0.5
+            cmd.V = 0.6 * sigmoid(step_acc, 0, 1)
             cmd.W = 0
         if current_operation == ManualOperationType.eRightMove:
             cmd.T = 2
-            cmd.V = -0.5
+            cmd.V = -0.6 * sigmoid(step_acc, 0, 1)
             cmd.W = 0
         cmd.L = self.L_
         cmd.end = 1
-        self.publish(self.speedCmdTopic, jtf.MessageToJson(cmd))
+
+        # 处理为json格式
+        cmd_dict = {}
+        for field in cmd.DESCRIPTOR.fields:
+            cmd_dict[field.name] = field.default_value
+        for field, value in cmd.ListFields():
+            cmd_dict[field.name] = value
+        cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
+        self.publish(self.speedCmdTopic, cmd_json)
+
+        print(cmd_json)
 
         # cmd_0 = message.ManualCmd()
         # cmd_0.key = str(uuid.uuid4())
@@ -593,6 +602,6 @@ class Robot(MqttAsync):
         # stub = msg_rpc.NavExcutorStub(channel)
         # response = stub.ManualOperation(cmd_0)
         # print("client received: ", response)
-        print(cmd)
+
         print("ManualOperation {op_type} task completed!!!".format(op_type=current_operation))
         return True

custom_define.py

@@ -0,0 +1,28 @@
+import enum
+
+
+class ActType(enum.IntEnum):
+    eReady = 0
+    eRotation = 1
+    eHorizon = 2
+    eVertical = 3
+    eMPC = 4
+    eClampOpen = 5
+    eClampClose = 6
+
+
+class ManualOperationType(enum.IntEnum):
+    eReady = 0x0000
+
+    eCounterclockwiseRotate = 0x0001
+    eClockwiseRotate = 0x0002
+
+    eForwardMove = 0x0010
+    eBackwardMove = 0x0020
+    eLeftMove = 0x0040
+    eRightMove = 0x0080
+
+
+class RobotName:
+    strAGVMain = "AgvMain"
+    strAGV2 = "AGV2"

message.proto

@@ -13,6 +13,8 @@ message AgvStatu{
   float w=2;
   int32 clamp=3;    //夹持状态  0，中间状态，1夹紧到位，2，打开到位，
   int32 clamp_other=4; //另外一节
+  int32 lifter=5;
+  int32 lifter_other=6;
 
 }
 
@@ -89,7 +91,7 @@ message NavStatu
 {
   int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关
   bool main_agv=2; //是否是两节控制plc
-  int32 move_mode=3; //运动模式，1：single，2：双车
+  int32 move_mode=3; //运动模式，0：single，1：双车
   LidarOdomStatu odom=4;
   string key = 5; // 任务唯一码
   Trajectory selected_traj = 6;

mqtt_async.py

@@ -1,11 +1,15 @@
 from paho.mqtt import client as mqtt_client
 from threading import Lock
+
+
 class MqttAsync(object):
     def __init__(self):
-        self.lock_=Lock()
-        self.connected=False
-    def connect(self,client_id,ip,port,user,password):
-        self.client_id=client_id
+        self.lock_ = Lock()
+        self.connected = False
+
+    def connect(self, client_id, ip, port, user, password):
+        self.client_id = client_id
+
         def on_connect(client, userdata, flags, rc):
             if rc == 0:
                 print("Connected to MQTT Broker!")
@@ -16,21 +20,20 @@ class MqttAsync(object):
         self.client.username_pw_set(user, password)
         self.client.on_connect = on_connect
         self.client.connect(ip, port)
-        self.connected=True
+        self.connected = True
 
-    def publish(self,topic,msg):
-        if self.connected==False:
+    def publish(self, topic, msg):
+        if self.connected == False:
             print("mqtt is not connected Failed to send message to topic {topic}")
         with self.lock_:
-            result = self.client.publish(topic, msg,qos=1)
+            result = self.client.publish(topic, msg, qos=1)
         status = result[0]
         if status == 0:
             pass
         else:
             print(f"Failed to send message to topic {topic}")
 
-
-    def subscribe(self,topic,callback):
+    def subscribe(self, topic, callback):
         with self.lock_:
             self.client.subscribe(topic)
             self.client.on_message = callback
@@ -38,4 +41,3 @@ class MqttAsync(object):
     def loop_start(self):
         with self.lock_:
             self.client.loop_start()
-

mytool/db_helper/db_operation.py

@@ -0,0 +1,156 @@
+import sys
+
+sys.path.append("..")
+import mytool.db_helper.mysqlhelper as mdb
+
+
+# 数据库操作
+# 可自行添加操作函数
+
+class DBOperation:
+    def __init__(self,db_ip, db_port, db_name, db_user, db_password):
+        self._db = mdb.DB(db_ip, db_port, db_name, db_user, db_password)
+    # 查询
+    def query(self, sql):
+        return self._db.query_all_sql(sql)
+    # 更新
+    def update(self, sql):
+        return self._db.execute_sql(sql)
+    def query_parking_in_unit_tableid(self, unit,tableid):
+        sql = "select * from space WHERE unit=%s and table_id=%s"
+        return self._db.query_all_sql(sql, (unit,tableid))
+
+    def query_command_in_unit(self, unit):
+        sql = "select * from command_queue WHERE unit=%s"
+        return self._db.query_all_sql(sql, unit)
+
+    def query_pick_command_in_unit(self, unit):
+        sql = "select * from command_queue where unit=%s and type=2 order by queue_id ASC"
+        return self._db.query_all_sql(sql, unit)
+
+    def query_pick_command_in_unit_and_statu(self, unit, statu):
+        sql = "select * from command_queue where unit=%s and statu=%s and type=2 order by queue_id ASC"
+        return self._db.query_all_sql(sql, (unit, statu))
+
+    def query_sort_pick_command(self, unit):
+        sql = "select * from command_queue where unit=%s and (statu=0 or statu=1) and type=2 order by queue_id ASC"
+        return self._db.query_all_sql(sql, unit)
+    def query_command_all_in_unit_and_sort(self, unit):
+        sql = "select * from command_queue where unit=%s order by type,statu DESC,queue_id ASC"
+        return self._db.query_all_sql(sql, unit)
+
+    def query_command_all_in_unit_type_and_sort(self, unit, type):
+        sql = "select * from command_queue where unit=%s and type=%s order by type,statu DESC,queue_id ASC"
+        return self._db.query_all_sql(sql, (unit, type))
+    def query_command_all(self):
+        sql = "select * from command_queue"
+        return self._db.query_all_sql(sql)
+
+    def query_command_in_car_number(self, car_number):
+        sql = "select * from command_queue WHERE car_number=%s"
+        return self._db.query_all_sql(sql, car_number)
+
+    def query_command_in_primary_key(self, primary_key):
+        sql = "select * from command_queue WHERE primary_key=%s"
+        return self._db.query_all_sql(sql, primary_key)
+
+    def query_command_in_height_unit_and_statu(self, height, unit):
+        sql = "select * from command_queue WHERE height>=%s and unit=%s and statu=0 and type=1"  # type=1 存车
+        return self._db.query_all_sql(sql, (height, unit))
+    def query_command_in_height_minmax_unit_and_statu(self, min,max, unit):
+        sql = "select * from command_queue WHERE height>=%s and height<=%s and unit=%s and statu=0 and type=1"  # type=1 存车
+        return self._db.query_all_sql(sql, ( min,max, unit))
+    def query_space_in_car_number(self, car_number):
+        sql = "select * from space WHERE car_number=%s"
+        return self._db.query_all_sql(sql, car_number)
+    def query_space_in_primary_key(self, primary_key):
+        sql = "select * from space WHERE primary_key=%s"
+        return self._db.query_all_sql(sql, primary_key)
+
+    def query_space_in_unit_and_table_id(self, unit, table_id):
+        sql = "select * from space WHERE unit=%s and table_id=%s"
+        return self._db.query_all_sql(sql, (unit, table_id))
+    def query_space_in_height(self, height):
+        sql = "select * from space WHERE height>=%s"
+        return self._db.query_all_sql(sql, height)
+
+    def query_space_in_space_id(self, space_id):
+        sql = "select * from space WHERE id=%s"
+        return self._db.query_all_sql(sql, space_id)
+    def query_space_in_height_and_empty(self, height):
+        sql = "select * from space WHERE height>=%s and statu=0 and car_number IS NULL"
+        return self._db.query_all_sql(sql, height)
+
+    def query_space_in_height_unit_and_empty(self, height, unit):
+        sql = "select * from space WHERE height>=%s and unit=%s and statu=0 and car_number IS NULL"
+        return self._db.query_all_sql(sql, (height, unit))
+
+    def query_space_in_height_unit_and_empty_2(self, min,max, unit):
+        sql = "select * from space WHERE height>=%s and height<=%s and unit=%s and statu=0 and car_number IS NULL"
+        return self._db.query_all_sql(sql, (min, max, unit))
+
+    def query_space_in_unit(self, unit):
+        sql = "select * from space WHERE unit=%s"
+        return self._db.query_all_sql(sql, unit)
+
+    def query_space_all(self):
+        sql = "select * from space"
+        return self._db.query_all_sql(sql)
+    def query_vehicle_primary_key(self, car_number):
+        sql = "select primary_key from vehicle where car_number=%s"
+        return self._db.query_all_sql(sql, car_number)
+    def update_space_status_in_space_id(self, space_id, statu):
+        sql = "update space set statu=%s where id=%s"
+        return self._db.execute_sql(sql, (statu, space_id))
+
+    def update_space_status_car_number_in_space_id(self, space_id,car_number,statu):
+        sql = "update space set statu=%s,car_number=%s where id=%s"
+        return self._db.execute_sql(sql, (statu, car_number,space_id))
+
+    def update_command_status_in_car_number(self,car_number,statu):
+        sql = "update command_queue set statu=%s where car_number=%s"
+        return self._db.execute_sql(sql, (statu, car_number))
+    def update_space_remark_and_statu_in_space_id(self,space_id,remark,statu):
+        sql = "update space set remark=%s,statu=%s where id=%s"
+        return self._db.execute_sql(sql, (remark, statu,space_id))
+    def clear_space_data(self, space_id):
+        sql = "update space set car_number=NULL,primary_key=NULL,statu=0,space_info=NULL,measure_info=NULL,plate_color=NULL,plate_type=NULL,plate_confidence=NULL,recognition_time=NULL,plate_full_image=NULL,plate_clip_image=NULL,remark=NULL where id=%s"
+        return self._db.execute_sql(sql, space_id)
+
+
+
+    def delete_command(self, car_number):
+        sql = "delete from command_queue where car_number=%s"
+        return self._db.execute_sql(sql, car_number)
+
+    # 查询一段时间内停车次数
+    def query_park_count_in_time(self, in_date, out_date):
+        sql = "select * from record where in_time_start >= %s and in_time_start <= %s"
+        return self._db.query_all_sql(sql, (in_date,out_date))
+    def query_pick_count_out_time(self, in_date, out_date):
+        sql = "select * from record where out_time_start >= %s and out_time_start <= %s"
+        return self._db.query_all_sql(sql, (in_date, out_date))
+
+    def query_park_count_in_time_and_unit(self, in_date, out_date,unit):
+        sql = "select * from record where (in_time_start >= %s and in_time_start <= %s) and unit_id=%s"
+        return self._db.query_all_sql(sql, (in_date,out_date,unit))
+    def query_pick_count_out_time_and_unit(self, in_date, out_date,unit):
+        sql = "select * from record where (out_time_start >= %s and out_time_start <= %s) and unit_id=%s"
+        return self._db.query_all_sql(sql, (in_date, out_date,unit))
+
+     # 查询一段时间内停车车数
+    # period_type = 1 :当天内
+    #               2 :当月内
+    #               3 :自定义起止时间
+    def query_carQuantity_inPeriod(self, period_type, date):
+        sql = "select count(distinct(car_number)) from record where in_time_start like %s"
+        if period_type == "day":
+            date = date[0:10]
+        elif period_type == "month":
+            date = date[0:7]
+        date = date + "%"  # 模糊查询
+
+        result = self._db.query_all_sql(sql, date)  # [{'count(distinct(car_number))': 4}]
+
+        return result[0]["count(distinct(car_number))"]
+

mytool/db_helper/mysqlhelper.py

@@ -0,0 +1,233 @@
+#!/usr/bin/env python
+# -*- coding:utf-8 -*-
+# author:SunXiuWen
+# make_time:2019/1/13
+import logging
+import sys
+from time import sleep
+
+sys.path.append("..")
+import pymysql
+# 无法加载DButils包
+from DBUtils.PooledDB import PooledDB
+
+# from dbutils.pooled_db import PooledDB
+
+"""
+@功能：创建数据库连接池
+"""
+
+
+class DB(object):
+    """docstring for DbConnection"""
+    __pool = None
+
+    def __init__(self, db_ip, db_port, db_name, db_user, db_password):
+        self.db_ip = db_ip
+        self.db_port = db_port
+        self.db_name = db_name
+        self.db_user = db_user
+        self.db_password = db_password
+        self.pool = self.__get_conn_pool()
+
+    # 获取连接
+    def __get_conn_pool(self):
+        if self.__pool is None:
+            try:
+                self.__pool = PooledDB(creator=pymysql, host=self.db_ip, port=self.db_port,
+                                       user=self.db_user, passwd=self.db_password,
+                                       db=self.db_name, use_unicode=True, charset='utf8')
+            except Exception as e:
+                logging.error("%s : %s" % (Exception, e))
+        return self.__pool
+
+    # 获取连接
+    def _get_connection(self):
+        try:
+            conn = self.pool.connection()
+            cursor = conn.cursor(cursor=pymysql.cursors.DictCursor)
+            return conn, cursor
+        except Exception as e:
+            logging.error(" db.pool.connection ERROR: %s" % e)
+            return None, None
+
+    # 关闭连接
+    def _close_connection(self, conn, cursor):
+        if cursor:
+            cursor.close()
+        if conn:
+            conn.close()
+
+    # 查询
+    def query_all_sql(self, sql, param=None):
+        conn, cursor = self._get_connection()
+        try:
+            cursor.execute(sql, param)
+            result = cursor.fetchall()
+            self._close_connection(conn, cursor)
+        except Exception as e:
+            self._close_connection(conn, cursor)
+            logging.error(str(e))
+            result = None
+        return result
+
+    def query_one_sql(self, sql, param=None):
+        conn, cursor = self._get_connection()
+        try:
+            cursor.execute(sql, param)
+            result = cursor.fetchone()
+            self._close_connection(conn, cursor)
+        except Exception as e:
+            self._close_connection(conn, cursor)
+            logging.error(str(e))
+            result = None
+        return result
+
+    # 执行
+    def execute_sql(self, sql, param=None):
+        conn, cursor = self._get_connection()
+        try:
+            result = cursor.execute(sql, param)
+            conn.commit()
+            self._close_connection(conn, cursor)
+        except Exception as e:
+            conn.rollback()
+            self._close_connection(conn, cursor)
+            logging.error(str(e))
+            result = None
+        return result
+
+
+# class MySqLHelper(object):
+#     def __init__(self, db_ip, db_port, db_name, db_user, db_password):
+#         self.db = MyConnectionPool(db_ip, db_port, db_name, db_user, db_password)  # 从数据池中获取连接
+#
+#     # def __new__(cls, *args, **kwargs):
+#     #     if not hasattr(cls, 'inst'):  # 单例
+#     #         cls.inst = super(MySqLHelper, cls).__new__(cls, *args, **kwargs)
+#     #     return cls.inst
+#
+#     # 封装执行命令
+#     def execute(self, sql, param=None, autoclose=False):
+#         """
+#         【主要判断是否有参数和是否执行完就释放连接】
+#         :param sql: 字符串类型，sql语句
+#         :param param: sql语句中要替换的参数"select %s from tab where id=%s" 其中的%s就是参数
+#         :param autoclose: 是否关闭连接
+#         :return: 返回连接conn和游标cursor
+#         """
+#         cursor, conn = self.db.getconn()  # 从连接池获取连接
+#         count = 0
+#         try:
+#             # count : 为改变的数据条数
+#             if param:
+#                 count = cursor.execute(sql, param)
+#             else:
+#                 count = cursor.execute(sql)
+#             conn.commit()
+#             if autoclose:
+#                 self.close(cursor, conn)
+#         except Exception as e:
+#             print("db error_msg:", e.args)
+#         return cursor, conn, count
+#
+#     # 释放连接
+#     def close(self, cursor, conn):
+#         """释放连接归还给连接池"""
+#         cursor.close()
+#         conn.close()
+#
+#     # 查询所有
+#     def selectall(self, sql, param=None):
+#         try:
+#             cursor, conn, count = self.execute(sql, param)
+#             res = cursor.fetchall()
+#             return res
+#         except Exception as e:
+#             print("db error_msg:", e.args)
+#             self.close(cursor, conn)
+#             return count
+#
+#     # 查询单条
+#     def selectone(self, sql, param=None):
+#         try:
+#             cursor, conn, count = self.execute(sql, param)
+#             res = cursor.fetchone()
+#             self.close(cursor, conn)
+#             return res
+#         except Exception as e:
+#             print("db error_msg:", e.args)
+#             self.close(cursor, conn)
+#             return count
+#
+#     # 增加
+#     def insertone(self, sql, param):
+#         try:
+#             cursor, conn, count = self.execute(sql, param)
+#             conn.commit()
+#             self.close(cursor, conn)
+#             return count
+#         except Exception as e:
+#             print("db error_msg:", e.args)
+#             conn.rollback()
+#             self.close(cursor, conn)
+#             return count
+#
+#     # 删除
+#     def delete(self, sql, param=None):
+#         try:
+#             cursor, conn, count = self.execute(sql, param)
+#             conn.commit()
+#             self.close(cursor, conn)
+#             return count
+#         except Exception as e:
+#             print("db error_msg:", e.args)
+#             conn.rollback()
+#             self.close(cursor, conn)
+#             return count
+#
+#     # 修改
+#     def update(self, sql, param=None):
+#         try:
+#             cursor, conn, count = self.execute(sql, param)
+#             conn.commit()
+#             self.close(cursor, conn)
+#             return count
+#         except Exception as e:
+#             print("db error_msg:", e.args)
+#             conn.rollback()
+#             self.close(cursor, conn)
+#             return count
+#
+
+if __name__ == '__main__':
+    db = DB()
+    # while True:
+    #     sql1 = 'select * from space where unit=%s'
+    #     args = '11'
+    #     ret = db.query_sql(sql=sql1,param=args)
+    #     print(ret)  # (79, 2, 1, 2.2, None, 2, 1)
+    # 查询单条
+    sql1 = 'select * from space where unit=%s'
+    args = '11'
+    ret = db.query_one_sql(sql=sql1, param=args)
+    print(ret)  # (79, 2, 1, 2.2, None, 2, 1)
+    # 查询所有
+    sql2 = "select * from space"
+    ret = db.query_all_sql(sql=sql2)
+    print(ret)
+    # 增加
+    sql3 = 'insert into test VALUES (%s,%s,%s,%s)'
+    ret = db.execute_sql(sql3, ('鄂A6X3B0', 'DASDASDEFDFSDASDADASDAS', 1, 2))
+    print(ret)
+    # 删除
+    sql4 = 'delete from test WHERE car=%s'
+    args = '鄂A6X3B0'
+    ret = db.execute_sql(sql4, args)
+    print(ret)
+    sleep(0.1)
+    # 修改
+    sql5 = 'update test set ki=%s WHERE car=%s'
+    args = ('100', 'wkk')
+    ret = db.update_sql(sql5, args)
+    print(ret)

mytool/json_helper/parse_json.py

@@ -0,0 +1,16 @@
+import json
+# 读取带注释 // /* */ 的json文件
+def parse_json_with_comments(file_path):
+    # 指定文件编码为UTF-8
+    with open(file_path, 'r', encoding="UTF-8") as f:
+        lines = f.readlines()
+
+    # 从每行中删除注释
+    lines = [line.split('//')[0].strip() for line in lines]
+    lines = [line for line in lines if line]
+
+    #连接行以创建有效的JSON字符串
+    json_str = ''.join(lines)
+
+    # 加载JSON字符串
+    return json.loads(json_str)

mytool/log_helper/logger.py

@@ -0,0 +1,145 @@
+#!/usr/bin/python
+# -*- coding:utf-8 -*-
+
+# 1、日志默认的输出等级为：warning级别及以上的
+# 2、想修改日志的默认输出等级？通过logging.basicConfig(level="INFO")----info需要用大写
+# 3、想知道日志是什么时候打印出来的，以及其他参数？通过logging.basicConfig(format=console_fmt)
+# 4、如果要同时添加这2个参数，需要写在一行代码中，logging.basicConfig(level="INFO",format=console_fmt)
+# %(name)s：名字
+# %(levelname)s：日志级别
+# %(asctime)s：打印时间，年月日时分秒
+# %(message)s：日志中的信息
+# %(lineno)d：报错日志在代码中第几行
+# %(filename)s：文件名
+# %(thread)d：进程id
+# %(levelname)s：错误等级名称
+# '%(asctime)s - %(filename)s[line:%(lineno)d] %(thread)d- %(levelname)s: %(message)s'
+import logging
+import colorlog
+import os
+from logging.handlers import RotatingFileHandler
+from datetime import datetime
+
+log_colors_config = {
+    # 终端输出日志颜色配置
+    'DEBUG': 'white',
+    'INFO': 'green',
+    'WARNING': 'yellow',
+    'ERROR': 'red',
+    'CRITICAL': 'bold_red',
+}
+
+default_formats = {
+    # 终端输出格式
+    'console_format': '%(log_color)s%(asctime)s-id%(thread)d-%(filename)s[line:%(lineno)d]-%(levelname)s-[日志信息]: %(message)s',
+    # 日志输出格式
+    'log_format': '%(asctime)s-id %(thread)d-%(filename)s[line:%(lineno)d]-%(levelname)s-[日志信息]: %(message)s'
+}
+
+
+class HandleLog:
+    """
+    先创建日志记录器（logging.getLogger），然后再设置日志级别（logger.setLevel），
+    接着再创建日志文件，也就是日志保存的地方（logging.FileHandler），然后再设置日志格式（logging.Formatter），
+    最后再将日志处理程序记录到记录器（addHandler）
+    """
+
+    @staticmethod
+    def __init_logfile_handler(log_path):
+        """
+        创建日志记录器handler，用于收集日志
+        :param log_path: 日志文件路径
+        :return: 日志记录器
+        """
+        # 写入文件，如果文件超过 10M 大小时，切割日志文件，仅保留3个文件
+        logfile_handler = RotatingFileHandler(filename=log_path, maxBytes=10 * 1024 * 1024, backupCount=3,
+                                              encoding='utf-8')
+        return logfile_handler
+
+    @staticmethod
+    def __init_console_handle():
+        """创建终端日志记录器handler，用于输出到控制台"""
+        console_handle = logging.StreamHandler()
+        return console_handle
+
+    @staticmethod
+    def __set_handle(__logger, handle, level=logging.INFO):
+        """
+        设置handler级别并添加到终端logger收集器
+        :param handle: 终端日志记录器
+        :param level: 日志记录器级别
+        """
+        handle.setLevel(level=level)
+        __logger.addHandler(handle)
+
+    @staticmethod
+    def __set_logfile_formatter(file_handler):
+        """
+        设置日志输出格式-日志文件
+        :param file_handler: 日志记录器
+        """
+        formatter = logging.Formatter(default_formats["log_format"])
+        file_handler.setFormatter(formatter)
+
+    @staticmethod
+    def __set_console_formatter(console_handle):
+        """
+        设置输出格式-控制台
+        :param console_handle: 终端日志记录器
+        :return:
+        """
+        formatter = colorlog.ColoredFormatter(fmt=default_formats["console_format"], log_colors=log_colors_config)
+        console_handle.setFormatter(formatter)
+
+    @staticmethod
+    def __close_handler(file_handler):
+        """
+        关闭handler
+        :param file_handler: 日志记录器
+        """
+        file_handler.close()
+
+    @staticmethod
+    def get_logger(log_path='.\\log', is_save=True):
+        '''
+            log_path: 日志文件夹路径
+            is_save: 是否保存日志文件，默认保存
+        '''
+        __logger = logging.getLogger()  # 创建日志记录器
+        __logger.setLevel(logging.DEBUG)  # 设置默认日志记录器记录级别
+        # 创建终端日志记录器handler，用于输出到控制台
+        console_handle = HandleLog.__init_console_handle()
+        # 设置显示格式
+        HandleLog.__set_console_formatter(console_handle)
+        # 设置handler级别并添加到终端logger收集器
+        HandleLog.__set_handle(__logger, console_handle)
+
+        if is_save:
+            # 保存日志文件
+            # cur_path = os.path.dirname(os.path.realpath(__file__))  # 当前项目路径
+            # log_path = os.path.join(os.path.dirname(cur_path), log_path)  # log_path为存放日志的路径
+            if not os.path.exists(log_path): os.mkdir(log_path)  # 若不存在logs文件夹，则自动创建
+
+            _all_log_path = os.path.join(log_path, datetime.now().strftime('%Y-%m-%d') + "-all" + ".log")  # 收集所有日志信息文件
+            _error_log_path = os.path.join(log_path, datetime.now().strftime('%Y-%m-%d') + "-error" + ".log")  # 收集错误日志信息文件
+            #创建日志记录器handler，用于收集日志
+            all_logfile_handle = HandleLog.__init_logfile_handler(_all_log_path)
+            error_logfile_handle = HandleLog.__init_logfile_handler(_error_log_path)
+            # 设置显示格式
+            HandleLog.__set_logfile_formatter(all_logfile_handle)
+            HandleLog.__set_logfile_formatter(error_logfile_handle)
+            # 设置handler级别并添加到终端logger收集器
+            HandleLog.__set_handle(__logger, all_logfile_handle)
+            HandleLog.__set_handle(__logger, error_logfile_handle,level=logging.ERROR)
+
+        return __logger
+
+
+# if __name__ == '__main__':
+#     log = HandleLog.get_logger()
+#     log.info("这是日志信息")
+#     log.debug("这是debug信息")
+#     log.warning("这是警告信息")
+#     log.error("这是错误日志信息")
+#     log.critical("这是严重级别信息")
+

mytool/mqtt_helper/mqtt_client.py

@@ -0,0 +1,138 @@
+import json
+import threading
+import time
+
+from paho.mqtt import client as mqtt_communication
+
+# 状态消息定时器
+class TimeStatu:
+    def __init__(self, statu=None, timeout=3):
+        self.statu = statu
+        self.time = time.time()
+        self.timeout_ms = timeout
+    def timeout(self):
+        tm=time.time()
+        return tm-self.time>self.timeout_ms or self.statu==None
+# mqtt客户端
+class matt_client(threading.Thread):
+    # ip,端口,用户名（可选）,密码（可选）
+    def __init__(self, mqtt_ip, mqtt_port = 1883, mqtt_username=None, mqtt_passwd=None):
+        threading.Thread.__init__(self)
+        self.mqtt_ip = mqtt_ip
+        self.mqtt_port = mqtt_port
+        self.mqtt_username = mqtt_username
+        self.mqtt_passwd = mqtt_passwd
+        self.client = None
+        self.recv_topic_list = []
+        self.message_dict = {}
+        self.message_callback_dict = {}
+
+        # 连接mqtt默认回调函数
+        self.connect_callback = {}
+
+    # 绑定话题回调函数  若要绑定必须在run前调用
+    def bind_topic_callback(self, topic, callback):
+        self.message_callback_dict[topic] = callback
+
+    # 初始化 客户端id,接收状态列表 ps: ["topic1","topic2"]  ,连接回调默认为mqtt_connect_callback
+    def init(self,client_id, recv_topic_list,connect_callback = None):
+        # 默认连接回调
+        def on_connect(client, userdata, flags, rc):
+            if rc == 0:
+                print("Connected to MQTT Broker!")
+            else:
+                print("Failed to connect mqtt, return code %d\n", rc)
+        self.recv_topic_list = recv_topic_list
+        self.client = mqtt_communication.Client(client_id)
+        # 是否自定义连接回调
+        if connect_callback is not None:
+            self.client.on_connect = connect_callback
+        else:
+            self.client.on_connect = on_connect
+        # 是否有密码
+        if self.mqtt_username is not None:
+            self.client.username_pw_set(self.mqtt_username,self.mqtt_passwd)
+        # 连接
+        self.client.connect(self.mqtt_ip, self.mqtt_port)
+        if self.recv_topic_list is not None:
+            for topic in self.recv_topic_list:
+                self.message_dict[topic] = TimeStatu(None, 0.1)
+
+    # 发布消息
+    def publish(self,topic,message):
+        if self.client is None:
+            print("ERROR: client is not initialized!")
+        else:
+            return self.client.publish(topic, message)
+    # 获取状态消息
+    def _get_message_dict(self):
+        return self.message_dict
+    def __getitem__(self, topic):
+        return self.message_dict[topic]
+    def __setitem__(self, topic, value):
+        self.message_dict[topic]=value
+
+    def run(self):
+        # 默认消息回调
+        def on_message(client, userdata, msg):
+            print("on_message")
+            self.message_dict[msg.topic] = TimeStatu(msg.payload.decode(), 3)
+        # 绑定状态消息
+        if self.recv_topic_list is not None:
+            for topic in self.recv_topic_list:
+                print("bind topic:"+topic)
+                self.client.subscribe(topic)
+                self.client.on_message = on_message
+                # 如果自定义了回调函数则调用自定义的  未自定义调用默认的
+                if topic in self.message_callback_dict.keys():
+                    self.client.message_callback_add(topic,self.message_callback_dict[topic])
+
+                self.message_dict[topic] = TimeStatu(None, 0.1)
+        self.client.loop_forever()
+
+    def close(self):
+        self.client.disconnect()
+
+
+topic_all = ["wk1","wk2","wk3"]
+
+
+def callback1(client, userdata, msg):
+    print(f"callback1   Received `{msg.payload.decode()}` from `{msg.topic}` topic")
+
+def callback2(client, userdata, msg):
+    print(f"callback2   Received `{msg.payload.decode()}` from `{msg.topic}` topic")
+
+
+if __name__ == '__main__':
+    # 初始化
+    client = matt_client("192.168.2.40")
+    # for topic in topic_all:
+
+    client.init("WK",topic_all)
+    # client.publish("test","hello")
+
+    for topic in topic_all:
+        if topic.find("1") >= 0:
+            client.bind_topic_callback(topic, callback1)
+        if topic.find("2") >= 0:
+            client.bind_topic_callback(topic, callback2)
+    # 启动
+    client.start()
+    client.join()
+    # time.sleep(1)
+    # close = False
+    # while(close is False):
+    #     for topic in topic_all:
+    #         if client[topic].statu is not None:
+    #             str = json.loads(client[topic].statu)
+    #             if str['msg'] == 'close':
+    #                 client.close()
+    #                 close = True
+    #                 break
+    #             print(str)
+    #         print(client[topic].timeout())
+    #     time.sleep(1)
+
+
+

sigmoid.py

@@ -1,17 +1,17 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
-# f(x) = 1/(1+e(-z))
 
-'''
-min_value: y最小值
-max_value: y最大值
-right_offset: 曲线右移
-span: y从接近最小到接近最大时,自变量x的跨度
-'''
+# f(x) = 1/(1+e(-z))
 
 
 def sigmoid(x, min_value=0.3, max_value=5, right_offset=5, span=10):
+    """
+    min_value: y最小值
+    max_value: y最大值
+    right_offset: 曲线右移
+    span: y从接近最小到接近最大时,自变量x的跨度
+    """
     # 右移
     x -= right_offset
     # 缩放
@@ -41,4 +41,12 @@ if __name__ == "__main__":
         x = i + 1
         print(x, sigmoid(x, span=10))
 
-
+# cd grpc
+# mkdir -p cmake/build
+# pushd cmake/build
+# cmake -DgRPC_INSTALL=ON \
+#     -DgRPC_BUILD_TESTS=OFF \
+#     ../..
+# make -j 4
+# make install
+# popd

test.py

@@ -49,6 +49,9 @@ import message_pb2 as message
 import google.protobuf.json_format as jtf
 import uuid
 
+from custom_define import RobotName
+
+
 #===============================================================================
 
 
@@ -64,25 +67,27 @@ class MainWindow(QMainWindow):
         self.basic()
         self.count_frame_=ControllWidget.CountFrame()
         self.LoadMap()
-        config1={"label":"AgvMain",
-                 "rpc":"127.0.0.1:9090",
+        config1={"label":RobotName.strAGVMain,
+                 "rpc":"192.168.0.70:9090",
+                 # "rpc": "127.0.0.1:9090",
                  "street_nodes":self.ui_nodes["street_nodes"],
                  "space_nodes":self.ui_nodes["space_nodes"],
-                 "mqtt":["pyui1","192.168.0.70",1883,"admin","zx123456"],
+                 # "mqtt":["pyui1","192.168.0.70",1883,"admin","zx123456"],
                  # "mqtt":["pyui1","192.168.225.224",1883,"admin","zx123456"],
-                 # "mqtt":["pyui-main","127.0.0.1",1883,"admin","zx123456"],
+                 "mqtt":["pyui-main","127.0.0.1",1883,"admin","zx123456"],
                  "subTopics":{"agv_main/agv_statu":message.RobotStatu.__name__,
                               "agv_main/nav_statu":message.NavStatu.__name__},
                  "cmdTopic":"agv_main/nav_cmd",
                  "robotColor":[0.7,0.2,0.3]}
 
-        config2={"label":"AGV2",
-                 "rpc":"127.0.0.1:9091",
+        config2={"label":RobotName.strAGV2,
+                 # "rpc":"192.168.0.71:9090",
+                 "rpc": "127.0.0.1:9091",
                  "street_nodes":self.ui_nodes["street_nodes"],
                  "space_nodes":self.ui_nodes["space_nodes"],
-                 "mqtt":["pyui2","192.168.0.71",1883,"admin","zx123456"],
+                 # "mqtt":["pyui2","192.168.0.71",1883,"admin","zx123456"],
                  # "mqtt":["pyui2","192.168.225.224",1883,"admin","zx123456"],
-                 # "mqtt":["pyui-child","127.0.0.1",1883,"admin","zx123456"],
+                 "mqtt":["pyui-child","127.0.0.1",1883,"admin","zx123456"],
                  "subTopics":{"agv_child/agv_statu":message.RobotStatu.__name__,
                               "agv_child/nav_statu":message.NavStatu.__name__},
                  "cmdTopic":"agv_child/nav_cmd",