|
@@ -2,6 +2,8 @@ import copy
|
|
|
import enum
|
|
|
import time
|
|
|
import message_pb2 as message
|
|
|
+import grpc
|
|
|
+import message_pb2_grpc as msg_rpc
|
|
|
from mqtt_async import MqttAsync
|
|
|
import google.protobuf.json_format as jtf
|
|
|
from dijkstra.Map import DijikstraMap, SpaceNode, StreetNode
|
|
@@ -35,11 +37,12 @@ class TimeStatu:
|
|
|
|
|
|
class Robot(MqttAsync):
|
|
|
|
|
|
- def __init__(self, name=""):
|
|
|
+ def __init__(self,rpc_ipport,name=""):
|
|
|
MqttAsync.__init__(self)
|
|
|
self.timedRobotStatu_ = TimeStatu(message.RobotStatu, 0)
|
|
|
self.timedNavStatu_ = TimeStatu(message.NavStatu, 0)
|
|
|
self.dataTopic_ = {}
|
|
|
+ self.rpc_ipport_=rpc_ipport
|
|
|
self.messageArrivedSignal = None
|
|
|
self.currentNavData_ = None
|
|
|
self.navCmdTopic_ = None
|
|
@@ -182,13 +185,6 @@ class Robot(MqttAsync):
|
|
|
self.GeneratePath(begId, targetId, task_type=task_type)
|
|
|
# self.ExecPathNodes(autoDirect)
|
|
|
self.ExecNavtask(autoDirect)
|
|
|
- while self.IsNavigating() == True:
|
|
|
- '''if self.Connected() == False:
|
|
|
- print("robot disconnected cancel task")
|
|
|
- self.CancelNavTask()
|
|
|
- return False'''
|
|
|
- time.sleep(0.5)
|
|
|
- print(" Nav completed!!!")
|
|
|
|
|
|
if self.IsMainModeStatu():
|
|
|
Count.TestCount().loadCountAdd()
|
|
@@ -292,12 +288,16 @@ class Robot(MqttAsync):
|
|
|
print("Nav cmd is None")
|
|
|
return False
|
|
|
|
|
|
- count = 3
|
|
|
- while self.ActionType() == ActType.eReady and count > 0:
|
|
|
- self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
|
|
|
- count -= 1
|
|
|
- time.sleep(1)
|
|
|
- print("send nav cmd completed!!!")
|
|
|
+ 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:
|
|
|
+ print("nav failed :%s"%(response.info))
|
|
|
+ else:
|
|
|
+ print("nav completed !!!")
|
|
|
+
|
|
|
return True
|
|
|
|
|
|
def CreateNavPathNodesAction(self, path, autoDirect):
|
|
@@ -348,14 +348,9 @@ class Robot(MqttAsync):
|
|
|
cmd.newActions.add().CopyFrom(action)
|
|
|
|
|
|
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!!!")
|
|
|
+ channel=grpc.insecure_channel(self.rpc_ipport_)
|
|
|
+ stub = msg_rpc.NavExcutorStub(channel)
|
|
|
+ response = stub.Start(cmd)
|
|
|
return True
|
|
|
|
|
|
'''
|
|
@@ -447,17 +442,9 @@ class Robot(MqttAsync):
|
|
|
if mode == 2:
|
|
|
action.wheelbase = wheelbase
|
|
|
cmd.newActions.add().CopyFrom(action)
|
|
|
- loop = 3
|
|
|
- while loop > 0:
|
|
|
- if mode == 2:
|
|
|
- if self.IsMainModeStatu():
|
|
|
- return True
|
|
|
- if mode == 1:
|
|
|
- if self.IsMainModeStatu() == False:
|
|
|
- return True
|
|
|
- self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
|
|
|
- loop -= 1
|
|
|
- time.sleep(0.5)
|
|
|
+ channel=grpc.insecure_channel(self.rpc_ipport_)
|
|
|
+ stub = msg_rpc.NavExcutorStub(channel)
|
|
|
+ response = stub.Start(cmd)
|
|
|
return False
|
|
|
|
|
|
def IsClampClosed(self):
|
|
@@ -504,13 +491,9 @@ class Robot(MqttAsync):
|
|
|
act = message.NewAction()
|
|
|
act.type = 6
|
|
|
cmd.newActions.add().CopyFrom(act)
|
|
|
- published = False
|
|
|
- while self.IsClampClosed() == False:
|
|
|
- if self.ActionType() == ActType.eClampClose:
|
|
|
- published = True
|
|
|
- if published == False:
|
|
|
- self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
|
|
|
- time.sleep(0.5)
|
|
|
+ channel=grpc.insecure_channel(self.rpc_ipport_)
|
|
|
+ stub = msg_rpc.NavExcutorStub(channel)
|
|
|
+ response = stub.Start(cmd)
|
|
|
return True
|
|
|
|
|
|
def ClampOpen(self):
|
|
@@ -524,21 +507,19 @@ class Robot(MqttAsync):
|
|
|
act = message.NewAction()
|
|
|
act.type = 7
|
|
|
cmd.newActions.add().CopyFrom(act)
|
|
|
- published = False
|
|
|
- while self.IsClampOpened() == False:
|
|
|
- if self.ActionType() == ActType.eClampOpen:
|
|
|
- published = True
|
|
|
- if published == False:
|
|
|
- self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
|
|
|
- time.sleep(0.5)
|
|
|
+ channel=grpc.insecure_channel(self.rpc_ipport_)
|
|
|
+ stub = msg_rpc.NavExcutorStub(channel)
|
|
|
+ response = stub.Start(cmd)
|
|
|
return True
|
|
|
|
|
|
def CancelNavTask(self):
|
|
|
cmd = message.NavCmd()
|
|
|
cmd.action = 3
|
|
|
|
|
|
- while self.IsNavigating() == True:
|
|
|
- self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
|
|
|
- time.sleep(1)
|
|
|
+ channel=grpc.insecure_channel(self.rpc_ipport_)
|
|
|
+ stub = msg_rpc.NavExcutorStub(channel)
|
|
|
+
|
|
|
+ response = stub.Cancel(cmd)
|
|
|
+ print("client received: ", response)
|
|
|
print(" Cancel task completed!!!")
|
|
|
return True
|