def mouseReleaseEvent (self, e):
if self.SETMODE == None:
return
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable,False)
x = self.startPos.x() * self.resolution
y = self.startPos.y() * self.resolution
theta = math.atan2( e.pos().y() - self.startPos.y(), \
e.pos().x() - self.startPos.x() )
try:
if self.SETMODE == self.POSE:
pos = stdmsg.Pose()
pos.position.x = x
pos.position.y = y
pos.orentation.yaw = theta
print 'initial_pose is :', x,y,theta
if(rpc!= None):rpc.call('initial_pose', pos )
else: print 'rpc not set'
elif self.SETMODE == self.GOAL:
pos = stdmsg.Pose()
pos.position.x = x
pos.position.y = y
pos.orentation.yaw = theta
print 'initial_pose is :', x,y,theta
if(rpc!= None):rpc.call('set_goal', pos )
else: print 'rpc not set'
except e:
print e, 'socket error!'
self.SETMODE = None
def __init__(self, scan, resolution = 0.1):
super(LaserScan, self).__init__()
self.setAcceptDrops(False)
global flag
if (flag == False):
pos = stdmsg.Pose()
try:
f2 = open('../robot.txt')
via1 = f2.read()
via1 = via1.split(',')
x1 = float(via1[0])
y1 = float(via1[1])
theta1 = float(via1[2])
f2.close()
except:
x1=0
y1=0
theta1=0
f = open('../robot.txt', 'r+')
mat = "%.3f,%.3f,%.3f," % (0, 0, 0)
f.write(mat)
f.close()
pos.position.x = x1
pos.position.y = y1
pos.orentation.yaw = theta1
if rpc != None:
rpc.call('initial_pose', pos)
else:
print 'rpc not set'
flag = True
self.scan = scan
self.resolution = resolution
self.normalize()
def go_org(self, index):
# self.mappingset.currentindex()
# QtGui.QComboBox.currentIndex()+1
# getcombo = QtGui.QComboBox
## pos = stdmsg.Pose()
## f2 = open('origin.txt')
## via = f2.read()
## via = via.split(',')
## x1 = float(via[0])
## y1 = float(via[1])
## theta1 = float(via[2])
## f2.close()
## pos.position.x = x1
## pos.position.y = y1
## pos.orentation.yaw = theta1
## print 'initial_pose is :', x1, y1, theta1
## if rpc != None:
## rpc.call('initial_pose', pos)
## else:
## print 'rpc not set'
pos = stdmsg.Pose()
mappingset = "../mapping_point.ini"
mapRect = ConfigParser.ConfigParser()
mapRect.read(mappingset)
liststr = mapRect.sections()
if len(liststr) >= 1 and liststr[index] == 'pos1': ##匹配点1
X = float(mapRect.get("pos1", "x"))
Y = float(mapRect.get("pos1", "y"))
Qt = float(mapRect.get("pos1", "th"))
if len(liststr) >= 1 and liststr[index] == 'pos2': ##匹配点2
X = float(mapRect.get("pos2", "x"))
Y = float(mapRect.get("pos2", "y"))
Qt = float(mapRect.get("pos2", "th"))
if len(liststr) >= 1 and liststr[index] == 'pos3': ##匹配点3
X = float(mapRect.get("pos3", "x"))
Y = float(mapRect.get("pos3", "y"))
Qt = float(mapRect.get("pos3", "th"))
if len(liststr) >= 1 and liststr[index] == 'pos4': ##匹配点4
X = float(mapRect.get("pos4", "x"))
Y = float(mapRect.get("pos4", "y"))
Qt = float(mapRect.get("pos4", "th"))
if len(liststr) >= 1 and liststr[index] == 'pos5': ##匹配点5
X = float(mapRect.get("pos5", "x"))
Y = float(mapRect.get("pos5", "y"))
Qt = float(mapRect.get("pos5", "th"))
if len(liststr) >= 1 and liststr[index] == 'pos6': ##匹配点6
X = float(mapRect.get("pos6", "x"))
Y = float(mapRect.get("pos6", "y"))
Qt = float(mapRect.get("pos6", "th"))
pos.position.x = X
pos.position.y = Y
pos.orentation.yaw = Qt
print 'the current_pose is :', X, Y, Qt
if rpc != None:
rpc.call('initial_pose', pos)
else:
print 'rpc not set'
def saveMessage(self):
if not self.is_save:
self.is_save = True
self.parentMain.ui.saveMessage.setText(u"停止并保存记录数据")
ppos = stdmsg.Pose()
ppos.position.x = 0
ppos.position.y = 0
ppos.orentation.yaw = 0
rpc.call('saveMessage', ppos)
else:
self.is_save = False
self.parentMain.ui.saveMessage.setText(u"开始记录数据")
ppos = stdmsg.Pose()
ppos.position.x = 1
ppos.position.y = 0
ppos.orentation.yaw = 0
rpc.call('saveMessage', ppos)
def decelerate(self):
ppos = stdmsg.Pose()
ppos.position.x = 0
ppos.position.y = 0
ppos.orentation.yaw = 0
if rpc != None:
rpc.call('Decelerate', ppos)
else:
print 'rpc not set'
def backward(self):
ppos = stdmsg.Pose()
ppos.position.x = 0
ppos.position.y = 0
ppos.orentation.yaw = 0
if rpc != None:
rpc.call('Backward', ppos)
else:
print 'rpc not set'
def stop_and_play(self):
if not self.is_playing:
print("stop play")
self.is_playing = True
self.parentMain.ui.stopandplay.setText(u"播放")
ppos = stdmsg.Pose()
ppos.position.x = 0
ppos.position.y = 0
ppos.orentation.yaw = 0
rpc.call('StopAndPlay', ppos)
else:
print("play")
self.is_playing = False
self.parentMain.ui.stopandplay.setText(u"停止")
ppos = stdmsg.Pose()
ppos.position.x = 1
ppos.position.y = 0
ppos.orentation.yaw = 0
rpc.call('StopAndPlay', ppos)
def PlanHandler(self, plan):
print plan.goal_reached,
if plan.goal_reached and time.time() - self.goal_reached_time > 5:
self.goal_reached_time = time.time()
x, y, theta = self.autopath.next()
pos = stdmsg.Pose()
pos.position.x = x
pos.position.y = y
pos.orentation.yaw = theta
print 'goal_set is :', x, y, theta
self.rpc.call('set_goal', pos)
self.view.PlanHandler(plan)
def go_secondmatch2(self):
pos1 = stdmsg.Pose()
f2 = open('origin2.txt')
via = f2.read()
via = via.split(',')
x1 = float(via[0])
y1 = float(via[1])
theta1 = float(via[2])
f2.close()
pos1.position.x = x1
pos1.position.y = y1
pos1.orentation.yaw = theta1
print 'second match pose is :', x1, y1, theta1
if rpc != None:
rpc.call('initial_pose', pos1)
else:
print 'rpc not set'
def go_org(self):
# self.mappingset.currentindex()
# QtGui.QComboBox.currentIndex()+1
# getcombo = QtGui.QComboBox
pos = stdmsg.Pose()
f2 = open('origin.txt')
via = f2.read()
via = via.split(',')
x1 = float(via[0])
y1 = float(via[1])
theta1 = float(via[2])
f2.close()
pos.position.x = x1
pos.position.y = y1
pos.orentation.yaw = theta1
print 'initial_pose is :', x1, y1, theta1
if rpc != None:
rpc.call('initial_pose', pos)
else:
print 'rpc not set'