def followDirections(self):
txt = str(self.commandText.toPlainText())
print "txt", txt
if self.lcmApp.cur_pose != None:
print "loc", self.lcmApp.cur_pose[0:3]
loc_index, = kNN_index(
self.lcmApp.cur_pose[0:3],
transpose(
[self.m4du.tmap_locs_3d[x] for x in self.m4du.tmap_keys]),
1)
topo_key = self.m4du.tmap_keys[int(loc_index)]
if isChecked(self.useRobotYawBox):
yaw = self.lcmApp.cur_pose[3]
print "yaw", yaw
yaw = tklib_normalize_theta(yaw)
if yaw < 0:
yaw += 2 * math.pi
orient = yaw
print "yaw", math.degrees(yaw)
orients = [na.append(self.m4du.orients, 360.0)]
i_tmp, = kNN_index([math.degrees(orient)], orients, 1)
i_tmp = int(i_tmp % len(self.m4du.orients))
vp = str(topo_key) + "_" + str(self.m4du.orients[i_tmp])
vp_i = self.m4du.vpt_to_num[vp]
vps = [vp_i]
else:
vps = []
for orient in self.m4du.orients:
vp = str(topo_key) + "_" + str(orient)
vp_i = self.m4du.vpt_to_num[vp]
vps.append(vp_i)
else:
vps = None
self.path = self.modelBrowser.runSentence(txt, vps)
def shouldDrawText(self):
return qt_utils.isChecked(self.shouldDrawIconsCheckBox)
def shouldDrawObjectGeom(self):
return qt_utils.isChecked(self.shouldDrawObjectGeomCheckBox)
def shouldDrawAgent(self):
return qt_utils.isChecked(self.shouldDrawAgentCheckBox)