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()
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
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