def pauseSystem(self, pausing): learningStatus = Bool() controlStatus = Bool() #if not pausing: # self.expStatus = "monitor" #self.backToInit = not pausing learningStatus.data = not pausing self.learningMF_pub.publish(learningStatus) self.learningMB_pub.publish(learningStatus) self.learningMB2_pub.publish(learningStatus) controlStatus.data = not pausing self.control_pub.publish(controlStatus) self.askStopPlanning = not pausing planDecide = CommandSignal() planDecide.decide = not pausing self.plde_pub.publish(planDecide)
def reset(self): #print "Reset" # reset exp: pause learning and control, send goal to base, wait until it's reached #print rospy.Time.now(), rospy.get_time() #if self.backToInit: if self.expSubState == 0: print "Sending stop plan signal" # sending planning and deciding inhibition planDecide = CommandSignal() # planDecide.plan = False planDecide.decide = False self.plde_pub.publish(planDecide) #keypressed = "" #while not (keypressed == " "): # keypressed = raw_input() time.sleep(2) self.expSubState = 2 # elif self.expSubState == 1: # # publish reward message: # rwd = Float32() # rwd.data = self.rewardToSend # self.reward_pub.publish(rwd) # time.sleep(1) # self.expSubState = 2 elif self.expSubState == 2: print "Wait for action to finish ...", self.actionHasFinished # keypressed = "" #while not (keypressed == " "): # keypressed = raw_input() if self.actionHasFinished: self.expSubState = 3 elif self.expSubState == 3: print "Action has finished:", self.actionHasFinished, " with reward, computing goal and pausing" #randpose = self.initialPoses[np.random.randint(0, len(self.initialPoses))] randpose = self.initRandPoses.pop() # Compute angle: xr = self.robotpose[0] yr = self.robotpose[1] xg = float(randpose[0]) yg = float(randpose[1]) dx = xg - xr dy = yg - yr orientcos = dx / math.sqrt(dx * dx + dy * dy) if abs(dx+dy) > 0.0 else 1.0; orientsin = dy / math.sqrt(dx * dx + dy * dy) if abs(dx+dy) > 0.0 else 0.0; print "Rand pose:", randpose, "orient:", 180.0 * math.acos(orientcos) / 3.14159265359 , 180.0 * math.asin(orientsin) / 3.14159265359, 180.0 * math.atan2(dy, dx) / 3.14159265359 goalAngle = math.atan2(dy, dx) # Action lib goal print "AC goal" self.pauseSystem(True) self.goal = MoveBaseGoal() self.goal.target_pose.header.frame_id = self.goalTF self.goal.target_pose.header.stamp = rospy.Time.now() self.goal.target_pose.pose.position.x = float(randpose[0]) self.goal.target_pose.pose.position.y = float(randpose[1]) self.goal.target_pose.pose.orientation.x = 0.0 self.goal.target_pose.pose.orientation.y = 0.0 self.goal.target_pose.pose.orientation.z = math.sin(goalAngle / 2.0) self.goal.target_pose.pose.orientation.w = math.cos(goalAngle / 2.0) print "Goto :", self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y print "From: ", self.robotpose[0], self.robotpose[1] #print "Confirm to send goal :" #keypressed = "" #while not (keypressed == " "): # keypressed = raw_input() time.sleep(1) self.expSubState = 4 elif self.expSubState == 4: # self.client.send_goal(self.goal) print "Sending initial position and Waiting ..." self.client.send_goal_and_wait(self.goal) goalStatus = self.client.get_state() print "Got result !" print self.client.get_goal_status_text(), goalStatus, self.client.get_result() # keypressed = "" # while not (keypressed == " "): # keypressed = raw_input() # print "ok" if (goalStatus == actionlib.GoalStatus.SUCCEEDED): self.expSubState = 5 elif (goalStatus == actionlib.GoalStatus.ABORTED): self.expSubState = 6 #if self.client.get_state(): # self.expSubState = 4 #self.backToInit = False #self.actionHasFinished = False elif self.expSubState == 5: # SUCCEEDED dx = self.goal.target_pose.pose.position.x - self.robotpose[0] dy = self.goal.target_pose.pose.position.y - self.robotpose[1] dist = math.sqrt(dx*dx + dy*dy) print "SUCCESS ! ", dist, self.client.get_goal_status_text(), self.client.get_state(), self.client.get_result() if self.rwdAcc < self.rwdObj: print "Back to monitoring ..." self.pauseSystem(False) self.expState = 0 else: print "End of Experiment !" self.expState = 2 elif self.expSubState == 6: # ABORTED print "Something went wrong, check and eventually drive the robot manually and then press Space!" keypressed = "" # wait for user's manual action: while not (keypressed == " "): keypressed = raw_input() print "Ok" self.pauseSystem(False) self.expState = 0
def reset(self):
# print "Reset"
# reset exp: pause learning and control, send goal to base, wait until it's reached
# print rospy.Time.now(), rospy.get_time()
if self.backToInit:
if self.actionHasFinished:
print "Action has finished:", self.actionHasFinished, " with reward, sending initial position."
randpose = self.initialPoses[np.random.randint(0, len(self.initialPoses))]
# Compute angle:
xr = self.robotpose[0]
yr = self.robotpose[1]
xg = float(randpose[0])
yg = float(randpose[1])
dx = xg - xr
dy = yg - yr
orientcos = dx / math.sqrt(dx * dx + dy * dy) if abs(dx + dy) > 0.0 else 1.0
orientsin = dy / math.sqrt(dx * dx + dy * dy) if abs(dx + dy) > 0.0 else 0.0
print "Rand pose:", randpose, "orient:", 180.0 * math.acos(
orientcos
) / 3.14159265359, 180.0 * math.asin(orientsin) / 3.14159265359, 180.0 * math.atan2(
dy, dx
) / 3.14159265359
goalAngle = math.atan2(dy, dx)
# Action lib goal
print "AC goal"
self.pauseSystem(True)
self.goal = MoveBaseGoal()
self.goal.target_pose.header.frame_id = self.goalTF
self.goal.target_pose.header.stamp = rospy.Time.now()
self.goal.target_pose.pose.position.x = float(randpose[0])
self.goal.target_pose.pose.position.y = float(randpose[1])
self.goal.target_pose.pose.orientation.x = 0.0
self.goal.target_pose.pose.orientation.y = 0.0
self.goal.target_pose.pose.orientation.z = math.sin(goalAngle / 2.0)
self.goal.target_pose.pose.orientation.w = math.cos(goalAngle / 2.0)
# self.client.send_goal(self.goal)
print "Waiting ..."
# print self.client.get_goal_status_text(), self.client.get_state(), self.client.get_result()
self.client.send_goal_and_wait(self.goal)
print self.client.get_goal_status_text(), self.client.get_state(), self.client.get_result()
self.backToInit = False
self.actionHasFinished = False
print "Done"
# authorising experts to decide :
self.askStopPlanning = True
planDecide = CommandSignal()
planDecide.decide = True
self.plde_pub.publish(planDecide)
else:
pass
# sending planning and deciding inhibition
if self.askStopPlanning:
print "Waiting for action to finish ..."
planDecide = CommandSignal()
planDecide.decide = False
self.plde_pub.publish(planDecide)
self.askStopPlanning = False
else:
# self.client.wait_for_result(rospy.Duration(0.5))
# print self.client.get_result()
# Update expStatus:
dx = self.goal.target_pose.pose.position.x - self.robotpose[0]
dy = self.goal.target_pose.pose.position.y - self.robotpose[1]
dist = math.sqrt(dx * dx + dy * dy)
print dist, self.client.get_goal_status_text(), self.client.get_state(), self.client.get_result(), actionlib.GoalStatus.ABORTED, actionlib.GoalStatus.SUCCEEDED
goalStatus = self.client.get_state()
if goalStatus == actionlib.GoalStatus.SUCCEEDED:
if self.rwdAcc < self.rwdObj:
print "Back to monitoring ..."
self.pauseSystem(False)
else:
print "End of Experiment !"
self.expStatus = "end"
elif goalStatus == actionlib.GoalStatus.ABORTED:
print "Something went wrong, drive the robot manually and then press Space!"
keypressed = ""
# wait for user's manual action:
while not (keypressed == " "):
keypressed = raw_input()
print "Ok"
self.pauseSystem(False)
else:
print "Other status:", goalStatus
self.actionHasFinished = False
