def start(self):
self.launch.start()
time.sleep(10)
rospy.init_node('admin')
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_cb)
self.pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=3)
while not self.pose_pub.get_num_connections():
pass
#init_pose = [ 0.0453101396561, 0.00364243984222, 0.0, 0.0, 0.0, -0.032842760778, 0.999460531019]
init_pose = self.traj[self.waypt_idx]
print(init_pose)
# initialize position in the map.
ipose = PoseWithCovarianceStamped()
ipose.header.frame_id = "map"
ipose.pose.pose.position.x = init_pose[0]
ipose.pose.pose.position.y = init_pose[1]
ipose.pose.pose.orientation.z = init_pose[5]
ipose.pose.pose.orientation.w = init_pose[6]
self.pose_pub.publish(ipose)
while not payload.isReady():
pass
self.pose_pub.publish(ipose)
time.sleep(1)
self.run_thread = multiprocessing.Process(target=self.run, args=())
self.run_thread.daemon = True
self.run_thread.start()
#def payload_mon(self):
print('In main process')
#print(payload.isReady())
pause_pub = rospy.Publisher('/pauseAction', Byte, queue_size=1)
while payload.isReady():
#print(payload.isPaused())
if payload.isPaused():
pause_pub.publish(Byte(1))
else:
pause_pub.publish(Byte(0))
time.sleep(0.1)
def run(self):
s = 0
print('Run Thread Spawned!')
while payload.isReady():
#print('In While Loop...')
if payload.isRunning():
s = self.evaluate()
if s == 0:
s = self.gotoNext()
elif s == -128: # ending mark
payload.setDoneStatus()
break
if s != 0:
print('HELP NEEDED!')
# payload.sendSMS('FIXME: Robot got stuck, task is INCOMPLETE!')
# help code...
# THIS SEGMENT SERVES AS A TEMPLATE FOR NOW...
"""
def isReady(self):
return payload.isReady()