增加发送严格轨迹点导航指令

ControllWidget.py

@@ -222,7 +222,6 @@ class ControlFrame(QFrame):
             else:
                 print("agv not in main statu")
         else :
-
             self.threadPool_.submit(self.robot_.Navigatting,
                                     self.begId_, self.targetId_)
 

MapGLWidget.py

@@ -214,7 +214,7 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(pt7[0],pt7[1])
         glEnd()
         #绘制方向
-        self.drawAxis(pose,5,5)
+        self.drawAxis(pose,1.2,5)
         color=[1,1,1]
         self.DrawText([x-W/2,y-l/2],label,3,1.5,color)
 

RobotData.py

@@ -7,6 +7,7 @@ from dijkstra.Map import DijikstraMap,SpaceNode,StreetNode
 import uuid
 from auto_test import TestCount
 import random
+import math
 
 class ActType(enum.Enum):
     eReady=0
@@ -162,7 +163,8 @@ class Robot(MqttAsync):
     def Navigatting(self, begId, targetId):
         print("nav")
         self.GeneratePath(begId, targetId)
-        self.ExecNavtask()
+        self.ExecPathNodes()
+        #self.ExecNavtask()
         while self.IsNavigating() == True:
             '''if self.Connected() == False:
                 print("robot disconnected cancel task")
@@ -256,6 +258,101 @@ class Robot(MqttAsync):
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
+    def ExecPathNodes(self,autoDirect=False):
+        if self.navCmdTopic_ == None:
+            print("Robot has not set nav cmd topic")
+            return False
+        if not self.ActionType()==ActType.eReady:
+            print(" robot is navigating ,key:%s" % (self.timedNavStatu_.statu.key))
+            return False
+
+        cmd = message.NavCmd()
+        cmd.action=6
+        if self.currentNavPathNodes_ is not None and self.timedRobotStatu_.timeout() == False:
+            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.05
+                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.3
+                limitAdjustH.min=0.03
+                limitAdjustH.max=0.3
+
+                count=0
+                size=len(self.currentNavPathNodes_)
+                for i in range(size):
+                    node=self.currentNavPathNodes_[i]
+                    pose=message.Pose2d()
+                    accuracy=message.Pose2d()
+                    pose.x=node.x_
+                    pose.y=node.y_
+                    if i+1<size:
+                        next=self.currentNavPathNodes_[i+1]
+                        vector = [next.x_ - node.x_, next.y_ - node.y_]
+                        dx = vector[0]
+                        dy = vector[1]
+                        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
+                        pose.theta=yaw
+                    else:
+                        pose.theta=0
+
+                    if count==size-1:
+                        accuracy.x=0.02
+                        accuracy.y=0.02
+                        accuracy.theta=0.5*math.pi/(180.0)
+                    else:
+                        accuracy.x=0.05
+                        accuracy.y=0.1
+                        accuracy.theta=3*math.pi/180.0
+
+                    pathNode=message.PathNode()
+                    pathNode.pose.CopyFrom(pose)
+                    pathNode.accuracy.CopyFrom(accuracy)
+                    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)
+                cmd.newActions.add().CopyFrom(newAction)
+
+                count+=1
+
+            else:
+                print("statu is None")
+                return False
+        else:
+            print("current path is none")
+            return False
+        if cmd is None:
+            print("Nav cmd is None")
+            return False
+        print(cmd)
+        published=False
+        while self.IsNavigating() == False:
+            if not self.ActionType() == ActType.eReady:
+                published=True
+            if published==False:
+                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
+            time.sleep(1)
+        print("send nav cmd completed!!!")
+        return True
+
 
     def SwitchMoveMode(self, mode, wheelbase):
         if self.IsMainAgv() == False:

dijkstra/Map.py

@@ -166,7 +166,7 @@ class DijikstraMap(object):
 
         #过程点巡线目标精度
         node_mpcdiff.x=(0.05)
-        node_mpcdiff.y=(0.1)
+        node_mpcdiff.y=(0.05)
         node_mpcdiff.theta=(10 * math.pi / 180.0)
 
         #最后一个巡线目标点精度
@@ -253,9 +253,12 @@ class DijikstraMap(object):
             act_along.target.theta=(yaw)
             if count==len(path)-1:
                 act_along.target_diff.CopyFrom(enddiff)
-                act_along.velocity_limit.CopyFrom(last_MPC_v)
+
             else:
                 act_along.target_diff.CopyFrom(node_mpcdiff)
+            if isinstance(node, (SpaceNode)) or isinstance(last_node, (SpaceNode)):
+                act_along.velocity_limit.CopyFrom(last_MPC_v)
+            else:
                 act_along.velocity_limit.CopyFrom(mpc_x_limit)
             act_along.angular_limit.CopyFrom(mpc_angular_limit)
             cmd.actions.add().CopyFrom(act_along)

message.proto

@@ -39,6 +39,12 @@ message Pose2d
   float theta=3;
 }
 
+message PathNode  //导航路径点及精度
+{
+  Pose2d pose=1;    //路径点
+  Pose2d accuracy=2; //要求精度
+}
+
 message Trajectory
 {
   repeated Pose2d poses=1;
@@ -56,12 +62,31 @@ message Action
 }
 
 
+//----------------------------
+message NewAction //进库，出库，轨迹导航，夹持，松夹持
+{
+  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，5，夹持，6，松夹持，7，切换模式
+  PathNode begNode = 2; //进出库起点
+  PathNode passNode=3; //进出库途径点
+  PathNode targetNode = 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:切换双车模式
+}
+//-----------------------------
+
 message NavCmd
 {
-  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,6,新导航模式
   string key=2;
   float wheelbase=3;		//轴距
   repeated Action actions=4;
+  repeated NewAction newActions=5;
 }
 
 message NavStatu