|
@@ -274,7 +274,7 @@ class Robot(MqttAsync):
|
|
|
[type,nodes]=action
|
|
|
if type=="input":
|
|
|
[street,space]=nodes
|
|
|
- protoSpace=self.generateProtoNode(space,[0.02,0.02,0.5*math.pi/180.0])
|
|
|
+ protoSpace=self.generateProtoNode(space,[0.01,0.01,0.3*math.pi/180.0])
|
|
|
protoStreet=self.generateProtoNode(street,[0.02,0.02,1*math.pi/180.0])
|
|
|
act=message.NewAction()
|
|
|
act.type=1
|
|
@@ -309,12 +309,11 @@ class Robot(MqttAsync):
|
|
|
if cmd is None:
|
|
|
print("Nav cmd is None")
|
|
|
return False
|
|
|
- published=False
|
|
|
- while self.IsNavigating() == False:
|
|
|
- if not self.ActionType() == ActType.eReady:
|
|
|
- published=True
|
|
|
- if published==False:
|
|
|
- self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
|
|
|
+
|
|
|
+ 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!!!")
|
|
|
return True
|
|
@@ -364,6 +363,11 @@ class Robot(MqttAsync):
|
|
|
else:
|
|
|
pose.theta=0
|
|
|
|
|
|
+ if self.IsMainModeStatu() and count==0:
|
|
|
+ accuracy.x=0.05
|
|
|
+ accuracy.y=0.1
|
|
|
+ accuracy.theta=0.5*math.pi/(180.0)
|
|
|
+
|
|
|
if count==size-1:
|
|
|
accuracy.x=0.02
|
|
|
accuracy.y=0.02
|