pp.append(
[-40 + 10 * i, -40 + 10 * j, 0, -35 + 10 * i, -35 + 10 * j, 20])
# obstacle = np.concatenate((o1,o2,o3,o4,o5))
obstacle = np.asarray(pp)
xRange = 50
yRange = 50
zRange = 40
q_init = Node(-47, -47, 3)
q_goal = Node(20, 45, 1)
rrt_path = RRT(obstacle, q_init, q_goal, xRange, yRange, zRange)
path = rrt_path.main()
path.reverse()
map_current = 0
global reach
reach = 0
def sendPoint():
global reach, map_current, quad_at
point_publisher = rospy.Publisher('/iris/next_point',
PoseStamped,
queue_size=10)
progress_publisher = rospy.Publisher('/iris/progress',
Progress,
