Exemplo n.º 1
0
    def request_exploration(self):
        # exploration client
        client = actionlib.SimpleActionClient('explore_server',
                                              ExploreTaskAction)
        client.wait_for_server()

        # empty polygon
        polygonStamped = PolygonStamped()
        polygonStamped.header.frame_id = 'map'
        polygonStamped.header.stamp = rospy.Time.now()
        # starting point from turtlebot position
        initialGoal = PointStamped()
        initialGoal.header.frame_id = 'map'
        initialGoal.point = Point32(x=0, y=0, z=0)

        # unbounded exploration, uncomment for bounded
        # for x, y in zip(self.points_x, self.points_y):
        # polygonStamped.polygon.points.append(Point32(x=x, y=y, z=0))

        # setting exploration goal
        exploration_goal = ExploreTaskGoal()
        exploration_goal.explore_boundary = polygonStamped
        exploration_goal.explore_center = initialGoal

        # starting exploration
        client.send_goal(exploration_goal)

        rospy.loginfo("Exploration Started")
        client.wait_for_result()
Exemplo n.º 2
0
    def _create_initial_frontier_goal(self):
        frontier_gh = ExploreTaskGoal()
        curr_ts = rospy.Time.now()

        curr_header = Header()
        curr_header.frame_id = "map"
        curr_header.seq = 1
        curr_header.stamp = curr_ts
        
        # Point Stamped
        pt_s = PointStamped()
        pt_s.header = curr_header
        pt_s.point.x = 1.0
        # Polygon Stamped
        # Note that polygon is not defined so it's an unbounded exploration
        pg_s = PolygonStamped()
        pg_s.header = curr_header
        vertices = [(5.0, 5.0), (-5.0, 5.0), (-5.0, -5.0), (5.0, -5.0)]
        for vertex in vertices:
            pg_s.polygon.points.append(Point(x=vertex[0], y=vertex[1]))

        #frontier_gh.header = curr_header
        #frontier_gh.goal_id.stamp = curr_ts
        #frontier_gh.goal_id.id = 'initial_frontier_marco'
        frontier_gh.explore_boundary = pg_s
        frontier_gh.explore_center = pt_s
        
        rospy.loginfo(frontier_gh)  
        return frontier_gh  
Exemplo n.º 3
0
    def execute_inner(self, userdata):
        global ms
        self.last_mv = rospy.Time.now()

        rospy.loginfo('Beginning Exploration')
        client = SimpleActionClient('explore_server', ExploreTaskAction)
        rospy.loginfo('WAITING FOR EXPLORE SERVER...')
        client.wait_for_server()
        rospy.loginfo('EXPLORE SERVER IS NOW UP!')

        boundary = PolygonStamped()
        boundary.header.frame_id = "map"
        boundary.header.stamp = rospy.Time.now()
        r = userdata.boundary / 2.0
        rospy.loginfo('boundary : {}'.format(r))
        x, y, _ = userdata.initial_point

        boundary.polygon.points.append(Point(x + r, y + r, 0))
        boundary.polygon.points.append(Point(x - r, y + r, 0))
        boundary.polygon.points.append(Point(x - r, y - r, 0))
        boundary.polygon.points.append(Point(x + r, y - r, 0))

        center = PointStamped()
        center.header.frame_id = "map"
        center.header.stamp = rospy.Time.now()
        center.point.x = x
        center.point.y = y
        center.point.z = 0.0

        goal = ExploreTaskGoal()
        goal.explore_boundary = boundary
        goal.explore_center = center

        client.send_goal(goal)

        while True:
            if rospy.is_shutdown():
                exit()

            if client.wait_for_result(
                    rospy.Duration(0.3)):  # 0.3 sec. timeout to check for cans
                # The exploration node has finished
                res = client.get_state()
                rospy.loginfo('EXPLORE SERVER STATE:{}'.format(res))
                if res == 3:  ## SUCCEEDED
                    # if exploration is complete...
                    userdata.boundary += 1.0  # explore a larger area
                    return 'succeeded'  # finished! yay!
                else:
                    print client.get_goal_status_text()
                    print 'explore server failed : {}'.format(res)
                    # when explore server gives up, can't explore
                    return 'stuck'

            now = rospy.Time.now()

            if self.objective == 'discovery':
                t = ms.dis_data.time
                p = ms.dis_pt()
            elif self.objective == 'delivery':
                t = ms.del_data.time
                p = ms.del_pt()
            else:
                rospy.logerr('Invalid objective passed to Explore state')
                return 'aborted'

            # if we're here, exploration is not complete yet...
            if t != None:  # check initialized
                dt = (now - t).to_sec()
                discovered = (dt < 2.0)  # last seen within the last 2 seconds
                if discovered:
                    # if can was found ...
                    client.cancel_all_goals()
                    userdata.destination = p
                    return 'discovered'

            # more than 20 seconds have passed while completely still
            # we're probably stuck
            if (now - self.last_mv).to_sec() > 20.0:
                print 'haven\'t been moving for a while!'
                client.cancel_all_goals()
                return 'stuck'  # bad name... "stuck" would be better