def get_next_goal(goal):
global action_server, grid_map, sensor
feedback = irob_assignment_1.msg.GetNextGoalFeedback()
feedback.gain = 0
feedback.path = Path()
while grid_map is None:
if action_server.is_preempt_requested() or rospy.is_shutdown():
rospy.loginfo("Explorer: Preempted")
action_server.set_preempted()
return
rospy.loginfo("Explorer: No map recieved yet!")
rospy.sleep(1)
bbx_min = [
grid_map.info.origin.position.x, grid_map.info.origin.position.y
]
bbx_max = [
bbx_min[0] + (grid_map.info.width * grid_map.info.resolution),
bbx_min[1] + (grid_map.info.height * grid_map.info.resolution)
]
tf_buffer.can_transform(grid_map.header.frame_id, robot_frame_id,
rospy.Time(0), rospy.Duration(10))
# Get root position based on TF
robot_pose = tf_buffer.lookup_transform(grid_map.header.frame_id,
robot_frame_id, rospy.Time(0))
root_position = np.array([[
robot_pose.transform.translation.x, robot_pose.transform.translation.y
]])
tree = RRT(root_position, bbx_min, bbx_max, radius, extension_range)
clear_published_tree()
# Clear path
path = Path()
path.header.frame_id = grid_map.header.frame_id
path.header.stamp = rospy.Time.now()
best_branch_pub.publish(path)
success = True
i = 0
while i < max_nodes and not rospy.is_shutdown():
# Check if preempt has been requested by the client
if action_server.is_preempt_requested() or rospy.is_shutdown():
rospy.loginfo("Explorer: Preempted")
action_server.set_preempted()
success = False
break
node = tree.expand_tree(grid_map, sensor, l)
if node is None:
continue
i += 1
gain = node.gain_along_path(grid_map, sensor)
if gain > feedback.gain:
publish_tree(tree)
# Publish this new better path as feedback
feedback.gain = gain
rospy.loginfo("Explorer: Current max gain %s",
round(feedback.gain, 2))
rospy.loginfo("Explorer: %s nodes of %s, %s remaining", i,
max_nodes, max_nodes - i)
feedback.path = node.get_path(grid_map.header.frame_id)
feedback.path.header.frame_id = grid_map.header.frame_id
feedback.path.header.stamp = rospy.Time.now()
best_branch_pub.publish(feedback.path)
action_server.publish_feedback(feedback)
elif 0 == i % 100:
publish_tree(tree)
if success:
result = irob_assignment_1.msg.GetNextGoalResult()
result.gain = feedback.gain
result.path = feedback.path
result.path.header.frame_id = grid_map.header.frame_id
result.path.header.stamp = rospy.Time.now()
best_branch_pub.publish(result.path)
action_server.set_succeeded(result)
