예제 #1
0
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
	global robot_pose
	robot_pose = [None, init_pose]
    #rospy.init_node('ltl_planner_%s' %robot_name)
	print 'Robot %s: ltl_planner started!' %(robot_name)
    ###### publish to
    #----------
    #publish to
    #----------
	InitialPosePublisher = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size = 100)
  #   for i in xrange(10):
  #       SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
  #       rospy.sleep(0.1)
  #   print('Initial pose set to %s.' %str(init_pose))
	GoalPublisher = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size = 100)
	
	ang_pub = rospy.Publisher('mobile_base_controller/cmd_vel', Twist, queue_size = 100)
	ang = Twist()
	
    #----------
    #subscribe to
    #----------
	rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
    ####### robot information
	full_model = MotActModel(ts, act)
	planner = ltl_planner(full_model, robot_task, None)
    ####### initial plan synthesis
	planner.optimal(5)
    #######
	reach_xy_bound = 0.5 # m
	reach_yaw_bound = 0.1*PI # rad
	t0 = rospy.Time.now()
	while not rospy.is_shutdown():
		try:
			t = rospy.Time.now()-t0
			print '----------Time: %.2f----------' %t.to_sec()
			current_goal = planner.next_move

			if isinstance(current_goal, str):
				print 'the robot next_move is an action, currently not implemented for %s' %robot_name
				break

			print 'robot orientation %s' %str(robot_pose[1][2])
			if norm2(robot_pose[1][0:2], current_goal[0:2]) > reach_xy_bound:
				SendGoal(GoalPublisher, current_goal, t)
				print('Goal %s sent to %s.' %(str(current_goal),str(robot_name)))
				rospy.sleep(1)
			else:
				while  abs(abs(robot_pose[1][2])-current_goal[2]) > reach_yaw_bound:
					ang.angular.z = -5*(abs(robot_pose[1][2])-current_goal[2])
					print 'diff = %s' %str(abs(robot_pose[1][2])-current_goal[2])
					ang_pub.publish(ang)
				ang.angular.z = 0
				ang_pub.publish(ang)
				print('Goal %s reached by %s.' %(str(current_goal),str(robot_name)))
				planner.find_next_move()
				if current_goal == planner.next_move:
					break
		except rospy.ROSInterruptException:
			pass
예제 #2
0
    def human_region_cb(self, req=rqt_simulation_msgs.srv.RegionRequest()):

        region = req.region.data
        print self.header, 'Received new human region: ', region

        # Build hard task message
        self.hard_task = self.removeRegion(region)

        # Replan
        self.full_model = MotActModel(self.robot_motion, self.robot_action)
        self.planner = ltl_planner(self.full_model, self.hard_task,
                                   self.soft_task)
        ####### initial plan synthesis
        self.planner.optimal(10, 'static')

        rsp = rqt_simulation_msgs.srv.RegionResponse()
        return rsp
예제 #3
0
def planner(letter, ts, act, task):
    global header
    global POSE
    global c
    global confirm
    global object_name
    global region
    rospy.init_node('planner_%s' % letter)
    print 'Agent %s: planner started!' % (letter)
    ###### publish to
    activity_pub = rospy.Publisher('next_move_%s' % letter,
                                   activity,
                                   queue_size=10)
    ###### subscribe to
    rospy.Subscriber('activity_done_%s' % letter, confirmation,
                     confirm_callback)
    rospy.Subscriber('knowledge_%s' % letter, knowledge, knowledge_callback)
    ####### agent information
    c = 0
    k = 0
    flag = 0
    full_model = MotActModel(ts, act)
    planner = ltl_planner(full_model, None, task)
    #planner = ltl_planner(full_model, task, None)
    ####### initial plan synthesis
    planner.optimal(10)
    #######
    while not rospy.is_shutdown():
        next_activity = activity()
        ###############  check for knowledge udpate
        if object_name:
            # konwledge detected
            planner.update(object_name, region)
            print 'Agent %s: object incorporated in map!' % (letter, )
            planner.replan_simple()
            object_name = None
        ############### send next move
        next_move = planner.next_move
        next_state = planner.next_state
        ############### implement next activity
        if isinstance(next_move, str):
            # next activity is action
            next_activity.header = header
            next_activity.type = next_move
            next_activity.x = -0.76
            next_activity.y = 0.30
            print 'Agent %s: next action %s!' % (letter, next_activity.type)
            while not ((confirm[0] == next_move) and
                       (confirm[1] > 0) and confirm[2] == header):
                activity_pub.publish(next_activity)
                rospy.sleep(0.06)
            rospy.sleep(1)
            confirm[1] = 0
            header = header + 1
            print 'Agent %s: action %s done!' % (letter, next_activity.type)
        else:
            print 'Agent %s: next waypoint (%.2f,%.2f,%.2f)!' % (
                letter, next_move[0], next_move[1], next_move[2])
            while not ((confirm[0] == 'goto') and
                       (confirm[1] > 0) and confirm[2] == header):
                #relative_x = next_move[0]-POSE[0]
                #relative_y = next_move[1]-POSE[1]
                #relative_pose = [relative_x, relative_y]
                #oriented_relative_pose = rotate_2d_vector(relative_pose, -POSE[2])
                next_activity.type = 'goto'
                next_activity.header = header
                #next_activity.x = oriented_relative_pose[0]
                #next_activity.y = oriented_relative_pose[1]
                next_activity.x = next_move[0]
                next_activity.y = next_move[1]
                next_activity.psi = next_move[2]
                activity_pub.publish(next_activity)
                rospy.sleep(0.06)
            rospy.sleep(1)
            confirm[1] = 0
            header = header + 1
            print 'Agent %s: waypoint (%.2f,%.2f,%.2f) reached!' % (
                letter, next_move[0], next_move[1], next_move[2])
            planner.pose = [next_move[0], next_move[1]]
        planner.find_next_move()
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
    global robot_pose
    robot_pose = [None, init_pose]
    #global pose_goal
    rospy.init_node('ltl_planner_%s' % robot_name)
    print 'Robot %s: ltl_planner started!' % (robot_name)
    #----------
    #publish to
    #----------
    GoalPublisher = actionlib.SimpleActionClient(
        'move_base', move_base_msgs.msg.MoveBaseAction)
    GoalPublisher.wait_for_server()
    #----------
    #subscribe to
    #----------
    rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)

    ####### robot information
    full_model = MotActModel(ts, act)
    planner = ltl_planner(full_model, robot_task, None)
    ####### initial plan synthesis
    planner.optimal(10)
    #######
    t0 = rospy.Time.now()
    while not rospy.is_shutdown():
        try:
            t = rospy.Time.now() - t0
            new_goal = planner.next_move
            rospy.loginfo("NEW GOAL: " + str(new_goal))
            if new_goal == 'pick':
                tp = rospy.Time.now()
                data = open(
                    "../pick_test/" + str(rospy.get_param('test_name')) +
                    ".txt", "a")
                data.write("Time = " + str((tp - t0).to_sec()) +
                           ": Pick server starting\n")
                data.close()
                rospy.loginfo("Robot " + str(robot_name) +
                              " next action is action " + str(new_goal))
                rospy.loginfo("Waiting for pick Service...")
                # Pick object with Aruco marker
                rospy.wait_for_service('/pick_gui')
                rospy.loginfo("Service ready. Starting /pick_gui...")
                pickclient = rospy.ServiceProxy('/pick_gui', Empty)
                pickclient()
                data = open(
                    "../pick_test/" + str(rospy.get_param('test_name')) +
                    ".txt", "a")
                data.write("Time = " + str((rospy.Time.now() - t0).to_sec()) +
                           ": Pick server finished\n")
                data.write("Pick duration: " +
                           str((rospy.Time.now() - tp).to_sec()) + "\n")
                data.close()
                rospy.loginfo("Pick server finished. Duration: " +
                              str((rospy.Time.now() - tp).to_sec()))
                try:
                    aruco_detect = rospy.wait_for_message(
                        '/detected_aruco_pose', PoseStamped, timeout=0.5)
                except rospy.ROSException:
                    return
                #planner.find_next_move() #a effacer
            else:
                pose_goal = new_goal
                # move_base action server travels till (x,y) coordinates
                rospy.loginfo("Robot " + str(robot_name) + " moving to " +
                              str(pose_goal))
                SendGoal(pose_goal, rospy.Time.now() - t0, GoalPublisher)
                # the check_yaw function sets the orientation
                rospy.loginfo("Current robot position : " + str(robot_pose[1]))
                data = open(
                    "../pick_test/" + str(rospy.get_param('test_name')) +
                    ".txt", "a")
                if GoalPublisher.get_state() == 4:
                    rospy.logingo(
                        "Planning fail. Position not reached. Retrying...\n")
                    data.write("Planning fail. Position not reached.\n")
                else:
                    rospy.loginfo("Goal " + str(pose_goal) + " reached")
                data.write("Current robot position : " + str(robot_pose[1]) +
                           "\n")
                data.close()
            planner.find_next_move()  #indent -1
            if pose_goal == planner.next_move:
                data = open(
                    "../pick_test/" + str(rospy.get_param('test_name')) +
                    ".txt", "a")
                data.write("Mission duration : " +
                           str((rospy.Time.now() - t0).to_sec()) + "\n")
                data.close()
                rospy.loginfo("Mission duration : " +
                              str((rospy.Time.now() - t0).to_sec()))
                return
        except rospy.ROSInterruptException:
            data = open(
                "../pick_test/" + str(rospy.get_param('test_name')) + ".txt",
                "a")
            data.write("Stopped by user\n")
            data.close()
            pass
예제 #5
0
    def __init__(self):
        self.node_name = "LTL Planner"
        self.active = False
        self.beta = 10
        self.robot_pose = PoseWithCovarianceStamped()
        self.hard_task = ''
        self.soft_task = ''
        self.temporary_task_ = temporaryTask()
        self.robot_name = rospy.get_param('robot_name')
        self.agent_type = rospy.get_param('agent_type')
        scenario_file = rospy.get_param('scenario_file')
        self.replan_timer = rospy.Time.now()
        self.header = '[' + self.robot_name + '] '

        self.last_current_pose = PoseStamped()
        self.clear_costmap = rospy.ServiceProxy('move_base/clear_costmaps',
                                                Empty)

        robot_model = FTSLoader(scenario_file)
        [
            self.robot_motion, self.init_pose, self.robot_action,
            self.robot_task
        ] = robot_model.robot_model

        #-----------
        # Publishers
        #-----------
        self.InitialPosePublisher = rospy.Publisher('initialpose',
                                                    PoseWithCovarianceStamped,
                                                    queue_size=100)
        # Synthesised prefix plan Publisher
        self.PrefixPlanPublisher = rospy.Publisher('prefix_plan',
                                                   PoseArray,
                                                   queue_size=1)
        # Synthesised sufix plan Publisher
        self.SufixPlanPublisher = rospy.Publisher('sufix_plan',
                                                  PoseArray,
                                                  queue_size=1)
        # Goal pose for aerial vehicle
        if self.agent_type == 'aerial':
            self.GoalPublisher = rospy.Publisher('command/pose',
                                                 PoseStamped,
                                                 queue_size=1)

        #------------
        # Subscribers
        #------------
        if self.agent_type == 'ground':
            #localization_topic = 'amcl_pose'
            localization_topic = 'pose_gui'
        elif self.agent_type == 'aerial':
            localization_topic = 'ground_truth/pose_with_covariance'
        self.sub_amcl_pose = rospy.Subscriber(localization_topic,
                                              PoseWithCovarianceStamped,
                                              self.PoseCallback)
        # trigger start from GUI
        self.sub_active_flag = rospy.Subscriber('/planner_active', Bool,
                                                self.SetActiveCallback)
        # initial position from GUI
        self.sub_init_pose = rospy.Subscriber('init_pose', Pose,
                                              self.GetInitPoseCallback)
        # task from GUI
        self.sub_soft_task = rospy.Subscriber('soft_task', String,
                                              self.SoftTaskCallback)
        self.sub_hard_task = rospy.Subscriber('hard_task', String,
                                              self.HardTaskCallback)
        self.sub_temp_task = rospy.Subscriber('temporary_task', TemporaryTask,
                                              self.TemporaryTaskCallback)
        # environment sense
        self.sub_sense = rospy.Subscriber('/environment', Sense,
                                          self.SenseCallback)
        # clear costmap manually from GUI
        self.sub_clear = rospy.Subscriber('clear_costmap', Bool,
                                          self.ClearCallback)

        #------------
        # Services
        #------------
        rospy.Service(name='human_detected',
                      service_class=rqt_simulation_msgs.srv.Region,
                      handler=self.human_region_cb)

        ####### Wait 3 seconds to receive the initial position from the GUI
        usleep = lambda x: time.sleep(x)
        usleep(3)
        self.robot_motion.set_initial(self.init_pose)
        rospy.loginfo('%s : %s : The inital hard task is: %s' %
                      (self.node_name, self.robot_name, self.hard_task))
        rospy.loginfo('%s : %s : The inital soft task is: %s' %
                      (self.node_name, self.robot_name, self.soft_task))

        ####### robot information
        self.full_model = MotActModel(self.robot_motion, self.robot_action)
        self.planner = ltl_planner(self.full_model, self.hard_task,
                                   self.soft_task)
        ####### initial plan synthesis
        self.planner.optimal(10, 'static')
        ###### Store initial plan regions
        self.init_regions = self.getRegions(self.hard_task)

        # Unsubscribe topics - cleanest way out of here ahahahah
        self.sub_soft_task.unregister()
        self.sub_hard_task.unregister()
        self.sub_temp_task.unregister()

        ### Publish plan for GUI
        prefix_msg = self.plan_msg_builder(self.planner.run.line,
                                           rospy.Time.now())
        self.PrefixPlanPublisher.publish(prefix_msg)
        sufix_msg = self.plan_msg_builder(self.planner.run.loop,
                                          rospy.Time.now())
        self.SufixPlanPublisher.publish(sufix_msg)

        ### start up move_base
        self.navigation = actionlib.SimpleActionClient("move_base",
                                                       MoveBaseAction)
        self.navi_goal = GoalMsg = MoveBaseGoal()
예제 #6
0
edges = [(0, 1), (0, 2), (0, 4), (0, 9), (1, 4), (1, 2), (2, 4), (3, 11),
         (5, 8), (5, 11), (5, 12), (6, 11), (7, 9), (7, 12), (8, 12), (8, 11),
         (9, 12), (11, 12)]
edge_list = [(loc[e[0]], loc[e[1]]) for e in edges]
robot_motion.add_un_edges(edge_list, unit_cost=2)

##############################
# action FTS
############# no action model
action = dict()

robot_action = ActionModel(action)

robot_model = [robot_motion, init_pose, robot_action]
##############################
# complete robot model
robot_full_model = MotActModel(robot_motion, robot_action)

##############################
#specify soft and hard tasks

# case one
# hard_task = '(([]<> (r0 && <> (r8 && <> r7))) && ([]<> (r2 && <> r6)) && ([] !r5))'
# soft_task = '([]! c4)'

# case two
hard_task = '(([]<> r2) && ([]<> r3) && ([]<> r8))'
soft_task = '(([]<> r4) && ([]<> r6))'

sys_model = [robot_full_model, hard_task, soft_task]
예제 #7
0
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
    global robot_pose
    robot_pose = [None, init_pose]
    global pose_goal
    pose_goal = init_pose
    print 'Robot %s: ltl_planner started!' % (robot_name)
    #----------
    #publish to
    #----------
    InitialPosePublisher = rospy.Publisher('initialpose',
                                           PoseWithCovarianceStamped,
                                           queue_size=100)
    #   for i in xrange(10):
    #       SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
    #       rospy.sleep(0.1)
    #   print('Initial pose set to %s.' %str(init_pose))
    global GoalPublisher
    GoalPublisher = actionlib.SimpleActionClient(
        'move_base', move_base_msgs.msg.MoveBaseAction)
    GoalPublisher.wait_for_server()
    #----------
    ####### robot information
    full_model = MotActModel(ts, act)
    planner = ltl_planner(full_model, robot_task, None)
    ####### initial plan synthesis
    planner.optimal(5)
    #######
    #subscribe to
    #----------
    rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)

    t0 = rospy.Time.now()
    while not rospy.is_shutdown():
        try:
            t = rospy.Time.now() - t0
            print '----------Time: %.2f----------' % t.to_sec()
            new_goal = planner.next_move
            print 'New current goal : %s' % str(new_goal)
            if isinstance(new_goal, str):
                print 'the robot next_move is an action, currently not implemented for %s' % robot_name
                planner.find_next_move()
                if pose_goal == planner.next_move:
                    print 'next move : %s\nAction break' % str(
                        planner.next_move)
                    break
            else:
                pose_goal = new_goal
                print 'current yaw : %s' % str(abs(robot_pose[1][2]))
                # move_base action server travels till (x,y) coordinates
                SendGoal(pose_goal, t)
                GoalPublisher.wait_for_result()
                print '\\move_base Action server over\n-------------\nCurrent Goal : %s' % str(
                    pose_goal)
                print 'Current position : %s' % str(robot_pose[1])
                # check_yaw function set the orientation
                check_yaw(pose_goal[2])
                planner.find_next_move()
                if pose_goal == planner.next_move:
                    print 'next move : %s\nMove break' % str(planner.next_move)
                    break
        except rospy.ROSInterruptException:
            pass
예제 #8
0
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
    global robot_pose
    robot_pose = [None, init_pose]
    rospy.init_node('ltl_planner_%s' % robot_name)
    print 'Robot %s: ltl_planner started!' % (robot_name)
    ###### publish to
    #----------
    #publish to
    #----------
    InitialPosePublisher = rospy.Publisher('initialpose',
                                           PoseWithCovarianceStamped,
                                           queue_size=100)
    # for i in xrange(10):
    #     SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
    #     rospy.sleep(0.1)
    # print('Initial pose set to %s.' %str(init_pose))
    GoalPublisher = rospy.Publisher('move_base_simple/goal',
                                    PoseStamped,
                                    queue_size=100)

    ang_pub = rospy.Publisher('mobile_base_controller/cmd_vel',
                              Twist,
                              queue_size=100)
    ang = Twist()

    #----------
    #subscribe to
    #----------
    rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
    ####### robot information
    full_model = MotActModel(ts, act)
    planner = ltl_planner(full_model, robot_task, None)
    ####### initial plan synthesis
    planner.optimal(5)
    #######
    reach_xy_bound = 0.5  # m
    reach_yaw_bound = 0.1 * PI  # rad
    t0 = rospy.Time.now()
    while not rospy.is_shutdown():
        try:
            t = rospy.Time.now() - t0
            print '----------Time: %.2f----------' % t.to_sec()
            next_move = planner.next_move
            if isinstance(next_move, str):
                print 'Robot %s next move is action %s' % (str(robot_name),
                                                           str(next_move))
                client = SimpleActionClient('/play_motion', PlayMotionAction)
                print 'Waiting for Action Server...'
                client.wait_for_server()
                goal = PlayMotionGoal()
                goal.motion_name = next_move
                goal.skip_planning = False
                goal.priority = 0  # Optional
                print 'Sending actionlib goal with motion: %s' % next_move
                client.send_goal(goal)
                print 'Waiting for result...'
                action_ok = client.wait_for_result(rospy.Duration(30.0))
                state = client.get_state()
                if state >= 1:
                    print 'Successful execution of action %s' % next_move
                    planner.find_next_move()
                else:
                    print 'Failed execution of action %s. Will retry' % next_move
            else:
                print 'Robot %s next move is motion to %s' % (str(robot_name),
                                                              str(next_move))
                if ((norm2(robot_pose[1][0:2], next_move[0:2]) >
                     reach_xy_bound) or
                    (abs(robot_pose[1][2]) - next_move[2]) > reach_yaw_bound):
                    SendGoal(GoalPublisher, next_move, t)
                    print 'diff = %f' % (abs(robot_pose[1][2]) - next_move[2])
                    print('Goal %s sent to %s.' %
                          (str(next_move), str(robot_name)))
                    rospy.sleep(1)
                else:
                    print('Goal %s reached by %s.' %
                          (str(next_move), str(robot_name)))
                    #ang.angular.z = 0
                    #ang_pub.publish(ang)
                    planner.find_next_move()
                    if next_move == planner.next_move:
                        break
        except rospy.ROSInterruptException:
            pass