去掉精度及速度

RobotData.py

@@ -243,14 +243,10 @@ class Robot(MqttAsync):
             return ActType.eReady
 
     @staticmethod
-    def generateProtoNode(node, accuracy):
-        [dx, dy, dyaw] = accuracy
+    def generateProtoNode(node):
         protoNode = message.PathNode()
         protoNode.x = node.x_
         protoNode.y = node.y_
-        protoNode.l = dx
-        protoNode.w = dy
-        protoNode.theta = dyaw
         return protoNode
 
     def ExecNavtask(self, autoDirect):
@@ -259,61 +255,30 @@ class Robot(MqttAsync):
         cmd.key = key
         cmd.action = 0
 
-        limitMPC_v = message.SpeedLimit()
-        limitRotate = message.SpeedLimit()
-        limAdjustV = message.SpeedLimit()
-        limitAdjustH = message.SpeedLimit()
-        limitInOutV = message.SpeedLimit()
-        limitMPC_v.min = 0.03
-        limitMPC_v.max = 1.2
-        limitRotate.min = 3 * math.pi / 180.0
-        limitRotate.max = 25 * math.pi / 180.0
-        limAdjustV.min = 0.03
-        limAdjustV.max = 0.5
-        limitAdjustH.min = 0.03
-        limitAdjustH.max = 0.3
-        limitInOutV.min = 0.03
-        limitInOutV.max = 0.8
-
-        # if task_type == "DoubleAGV":
-        #     print("AdjustPath------------------")
-        #     if self.IsMainModeStatu() is False:
-        #         print("原始路径：")
-        #         for node in self.currentNavPathNodes_:
-        #             print(node.id_, node.y_)
-        #         self.currentNavPathNodes_ = self.AdjustPath(self.currentNavPathNodes_)
-        #         print("修改路径：")
-        #         for node in self.currentNavPathNodes_:
-        #             print(node.id_, node.y_)
-
         actions = self.SplitPath(self.currentNavPathNodes_)
         for action in actions:
             [type, nodes] = action
             if type == "input":
                 [street, space] = nodes
-                protoSpace = self.generateProtoNode(space, [0.05, 0.05, 0.7 * math.pi / 180.0])
-                protoStreet = self.generateProtoNode(street, [0.05, 0.05, 1 * math.pi / 180.0])
+                protoSpace = self.generateProtoNode(space)
+                protoStreet = self.generateProtoNode(street)
                 act = message.NewAction()
                 act.type = 1
+                act.wheelbase=2.78
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
-                act.NodeAngularLimit.CopyFrom(limitRotate)
-                act.InOutVLimit.CopyFrom(limitInOutV)
-                act.adjustVelocitylimit.CopyFrom(limAdjustV)
-                act.adjustHorizonLimit.CopyFrom(limitAdjustH)
+
                 cmd.newActions.add().CopyFrom(act)
             if type == "output":
                 [space, street] = nodes
-                protoSpace = self.generateProtoNode(space, [0.05, 0.05, 0.7 * math.pi / 180.0])
-                protoStreet = self.generateProtoNode(street, [0.05, 0.05, 1 * math.pi / 180.0])
+                protoSpace = self.generateProtoNode(space)
+                protoStreet = self.generateProtoNode(street)
                 act = message.NewAction()
                 act.type = 2
+                act.wheelbase=2.78
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
-                act.InOutVLimit.CopyFrom(limitInOutV)
-                act.NodeAngularLimit.CopyFrom(limitRotate)
-                act.adjustVelocitylimit.CopyFrom(limAdjustV)
-                act.adjustHorizonLimit.CopyFrom(limitAdjustH)
+
                 cmd.newActions.add().CopyFrom(act)
             if type == "nav":
                 action = self.CreateNavPathNodesAction(nodes, autoDirect)
@@ -344,62 +309,18 @@ class Robot(MqttAsync):
             statu = self.timedRobotStatu_.statu
             if statu is not None:
                 newAction = message.NewAction()
-                limitMPC_v = message.SpeedLimit()
-                limitRotate = message.SpeedLimit()
-                limAdjustV = message.SpeedLimit()
-                limitAdjustH = message.SpeedLimit()
-                limitMPC_v.min = 0.03
-                limitMPC_v.max = 1.5
-                limitRotate.min = 3 * math.pi / 180.0
-                limitRotate.max = 25 * math.pi / 180.0
-                limAdjustV.min = 0.03
-                limAdjustV.max = 0.3
-                limitAdjustH.min = 0.03
-                limitAdjustH.max = 0.3
-
-                count = 0
                 size = len(path)
                 for i in range(size):
                     node = path[i]
                     pathNode = message.PathNode()
                     pathNode.x = node.x_
                     pathNode.y = node.y_
-                    distance = 0
-                    if i + 1 < size:
-                        next = path[i + 1]
-                        vector = [next.x_ - node.x_, next.y_ - node.y_]
-                        dx = vector[0]
-                        dy = vector[1]
-                        distance = math.sqrt(dx * dx + dy * dy)
-                        yaw = math.asin(dy / math.sqrt(dx * dx + dy * dy))
-                        if yaw >= 0:
-                            if dx < 0:
-                                yaw = math.pi - yaw
-                        if yaw < 0:
-                            if dx < 0:
-                                yaw = -math.pi - yaw
-                        pathNode.theta = -yaw
-                    else:
-                        pathNode.theta = 0
-
-
-                    if self.IsMainModeStatu():
-                        pathNode.l = 0.15
-                        pathNode.w = 0.05
-                    else:
-                        pathNode.l = 0.3
-                        pathNode.w = 0.1
-
-                    count += 1
 
                     newAction.type = 4
                     if autoDirect:
                         newAction.type = 3
                     newAction.pathNodes.add().CopyFrom(pathNode)
-                    newAction.NodeVelocityLimit.CopyFrom(limitMPC_v)
-                    newAction.NodeAngularLimit.CopyFrom(limitRotate)
-                    newAction.adjustVelocitylimit.CopyFrom(limAdjustV)
-                    newAction.adjustHorizonLimit.CopyFrom(limitAdjustH)
+
                 return newAction
             else:
                 print("statu is None")

count.json

@@ -1 +1 @@
-{"load_count": 531, "single_count": 1146}
+{"load_count": 531, "single_count": 1254}

map.json

@@ -17,35 +17,26 @@
   },
   
   "space_nodes":{
-    "PW":[5.37,-14.34,-1.57],
-    "PWA":[5.37,-10,-1.57],
-    "I1":[34.39,16.27,0],
+    "PW":[5.37,-8.8,-1.57],
     "I":[33,16.27,0],
-    "I2":[31.61,16.27,0],
-    "P1":[2.94,-7.44,-1.57],
     "P":[2.94,-6.05,-1.57],
-    "P2":[2.94,-4.66,-1.57],
-    "PA1":[10.25,6.56,1.57],
     "PA":[10.25,5.17,1.57],
-    "PA2":[10.25,3.78,1.57],
-    "PX1":[20.43,-7.53,1.57],
-    "PX":[20.43,-6.14,1.57],
-    "PX2":[20.43,-4.75,1.57],
-    "PY1":[22.78,-7.53,1.57],
-    "PY":[22.78,-6.14,1.57],
-    "PY2":[22.78,-4.75,1.57]
+    "PB":[22.78,5.17,1.57],
+    "PX":[20.43,-6.14,0],
+    "PY":[22.78,-6.14,-1.57]
   },
   "roads":{
-    "road1":["I1","I","I2","H2"],
+    "road1":["I","H2"],
     "road2":["H1","N1","H2"],
     "road3":["V7","H2"],
     "conn":["V1","V2","V3","V4","V5","V6","N1","N2"],
-    "rp":["V4","PW","PWA"],
-    "vp1":["V1","P1","P","P2"],
-    "vp2":["V3","PA1","PA","PA2"],
+    "rp":["V4","PW"],
+    "vp1":["V1","P"],
+    "vp2":["V3","PA"],
     "s_conn_1":["V1","S1"],
     "s_conn_4":["V4","S4"],
-    "s_conn_5":["V5","PX1","PX","PX2"],
-    "s_conn_5":["V6","PY1","PY","PY2"]
+    "s_conn_5":["V5","PX"],
+    "CPY":["V6","PY"],
+    "CPB":["V6","PB"]
   }
 }

message.proto

@@ -26,12 +26,6 @@ message ToAgvCmd {
   int32 end=7;
 }
 
-message SpeedLimit
-{
-  float min=1;
-  float max=2;
-}
-
 message Pose2d
 {
   float x=1;
@@ -39,14 +33,14 @@ message Pose2d
   float theta=3;
 }
 
-
 message PathNode  //导航路径点及精度
 {
-  float x=1; //路径点
+  float x=1;    //目标点坐标
   float y=2;
-  float l=3; //要求精度范围
+  float l=3;    //目标点旋转矩形方程，代表精度范围
   float w=4;
   float theta=5;
+  string id=6;
 }
 
 message Trajectory
@@ -57,24 +51,19 @@ message Trajectory
 //----------------------------
 message NewAction //进库，出库，轨迹导航，夹持，松夹持
 {
-  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，6，夹持，7，松夹持，8，切换模式
+  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，6:夹持，7:松夹持，8:切换模式
   PathNode spaceNode = 2; //进出库起点
   PathNode passNode=3; //进出库途径点
   PathNode streetNode = 4; //进出库终点
   repeated PathNode pathNodes=5;//导航路径点
-  SpeedLimit InOutVLimit=6; //进出库速度
-  SpeedLimit NodeVelocityLimit=7; //马路点MPC速度限制
-  SpeedLimit NodeAngularLimit=8;  //马路点原地旋转速度限制
-  SpeedLimit adjustVelocitylimit=9; //马路点原地调整x速度
-  SpeedLimit adjustHorizonLimit=10;  //马路点原地横移速度限制
-  float wheelbase=11;		//切换模式，轴距信息
-  int32 changedMode=12; //1:切换单车模式，2:切换双车模式
+  float wheelbase=6;		//切换模式，轴距信息
+  int32 changedMode=7; //1:切换单车模式，2:切换双车模式
 }
 //-----------------------------
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel
+  int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
   string key=2;
   repeated NewAction newActions=5;
 }
@@ -84,10 +73,12 @@ 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：双车
-  string key = 4; // 任务唯一码
+  LidarOdomStatu odom=4;
+  string key = 5; // 任务唯一码
   Trajectory selected_traj = 6;
   Trajectory predict_traj = 7;
-
+  bool in_space=8;  //是否在车位/正在进入车位
+  string space_id=9;
 }
 message RobotStatu
 {

test.py

@@ -68,7 +68,7 @@ class MainWindow(QMainWindow):
                  "space_nodes":self.ui_nodes["space_nodes"],
                  # "mqtt":["pyui1","192.168.0.224",1883,"admin","zx123456"],
                  # "mqtt":["pyui1","192.168.225.224",1883,"admin","zx123456"],
-                 "mqtt":["pyui1","192.168.0.70",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",
@@ -79,7 +79,7 @@ class MainWindow(QMainWindow):
                  "space_nodes":self.ui_nodes["space_nodes"],
                  # "mqtt":["pyui2","192.168.0.224",1883,"admin","zx123456"],
                  # "mqtt":["pyui2","192.168.225.224",1883,"admin","zx123456"],
-                 "mqtt":["pyui1","192.168.0.71",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",