drones = []
drones_poses = []
for name in cf_names:
drone = Robot(name)
drones.append( drone )
drones_poses.append(drone.position())
obstacles = []
obstacles_poses = []
# obstacles_names = []
# obstacles_names = ['obstacle4', 'obstacle10', 'obstacle12', 'obstacle25']
# obstacles_names = ['obstacle25', 'obstacle10', 'obstacle12',]#, 'obstacle4']
# obstacles_names = ['obstacle10', 'obstacle13', 'obstacle15']
obstacles_names = ['obstacle13']
for name in obstacles_names:
obstacle = swarmlib.Obstacle(name)
obstacles.append( obstacle )
obstacles_poses.append(obstacle.position()[:2])
if __name__ == "__main__":
if data_recording:
print "Data recording started"
pose_recording = Process(target=start_recording, args=(cf_names + [drone_joystick_name], obstacles_names,))
pose_recording.start()
if toFly:
cf_list = []
for cf_name in cf_names:
# print "adding.. ", cf_name
cf = crazyflie.Crazyflie(cf_name, '/vicon/'+cf_name+'/'+cf_name)
if __name__ == '__main__':
rospy.init_node('fly_labirint', anonymous=True)
if toFly:
print "takeoff"
cf1 = crazyflie.Crazyflie(cf_names[0],
'/vicon/' + cf_names[0] + '/' + cf_names[0])
cf1.setParam("commander/enHighLevel", 1)
cf1.takeoff(targetHeight=TAKEOFFHEIGHT, duration=2.0)
time.sleep(TakeoffTime)
# Objects init
obstacles = np.array([])
for i in range(len(obstacle_names)):
obstacles = np.append(
obstacles, swarmlib.Obstacle(obstacle_names[i], i, R_obstacles))
drone1 = swarmlib.Drone(cf_names[0], obstacles)
drone2 = swarmlib.Drone(cf_names[1], obstacles)
# drone3 = swarmlib.Drone(cf_names[2], obstacles)
drone1.start = obstacles[0].position() + np.array([-1.0, 0, TAKEOFFHEIGHT])
drone1.goal = obstacles[0].position() + np.array([0.5, 0, TAKEOFFHEIGHT])
xs, ys, zs = drone1.start
xg, yg, zg = drone1.goal
N_samples = ViconRate * TimeFlightSec
# [X] [Y] [Z]
drone1.traj = np.array([
np.linspace(xs, xg, N_samples),
np.linspace(ys, yg, N_samples),
np.linspace(zs, zg, N_samples)
]).T
while not rospy.is_shutdown():
# TODO: update current position of all objects every loop
# print '\nhuman_pose', human.position()
# OBSTACLEs
# TODO: make it to look good, by Ruslan
# for i in range(len(obstacle)):
# drone1.sp, updated_1 = swarmlib.pose_update_obstacle(drone1, obstacle[i], R_obstacles)
# drone2.sp, updated_2 = swarmlib.pose_update_obstacle(drone2, obstacle[i], R_obstacles)
# drone3.sp, updated_3 = swarmlib.pose_update_obstacle(drone3, obstacle[i], R_obstacles)
for i in range(len(obstacle_names)):
obstacle = np.append(
obstacle,
swarmlib.Obstacle(obstacle_names[i],
np.array([drone1.sp, drone2.sp, drone3.sp])))
points[i] = obstacle[i].position()[:2]
hull = ConvexHull(points)
plt.plot(points[:, 0], points[:, 1], 'o')
for simplex in hull.simplices:
plt.plot(points[simplex, 0], points[simplex, 1], 'k-')
plt.show()
# TO VISUALIZE
human.publish_position()
drone1.publish_sp()
drone2.publish_sp()
drone3.publish_sp()
drone1.publish_path()
drone2.publish_path()
rospy.init_node('gradient_interactive', anonymous=True)
# Objects inititalization
human = swarmlib.Mocap_object(human_name)
swarm = []
for name in cf_names:
swarm.append(swarmlib.Drone(name))
swarm[0].leader = True
# Obstacles init
obstacles_poses = []
for name in obstacles_names:
obstacles_poses.append(swarmlib.Mocap_object(name).position()[:2])
obstacles_poses = np.array(obstacles_poses)
obstacles_array = []
for ind in range(len(obstacles_poses)):
obstacles_array.append(swarmlib.Obstacle('obstacle_%d' % ind))
obstacles_array[-1].pose = obstacles_poses[ind].tolist() + [
TakeoffHeight
]
obstacles_array[-1].R = R_obstacles
ind += 1
if toFly:
cf_list = []
for name in cf_names:
cf = crazyflie.Crazyflie(name, '/vicon/' + name + '/' + name)
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
cf_list.append(cf)
print "takeoff"
cf2 = crazyflie.Crazyflie(cf_names[1], '/vicon/'+cf_names[1]+'/'+cf_names[1])
cf2.setParam("commander/enHighLevel", 1)
cf2.setParam("stabilizer/estimator", 2) # Use EKF
cf2.setParam("stabilizer/controller", 2) # Use mellinger controller
cf2.takeoff(targetHeight = TakeoffHeight, duration = TakeoffTime)
cf3 = crazyflie.Crazyflie(cf_names[2], '/vicon/'+cf_names[2]+'/'+cf_names[2])
cf3.setParam("commander/enHighLevel", 1)
cf3.setParam("stabilizer/estimator", 2) # Use EKF
cf3.setParam("stabilizer/controller", 2) # Use mellinger controller
cf3.takeoff(targetHeight = TakeoffHeight, duration = TakeoffTime)
time.sleep(TakeoffTime+1)
# Objects init
obstacles = np.array([])
for i in range(len(obstacle_names)):
obstacles = np.append(obstacles, swarmlib.Obstacle( obstacle_names[i], i, R_obstacles))
drone1 = swarmlib.Drone(cf_names[0], obstacles)
drone2 = swarmlib.Drone(cf_names[1], obstacles)
drone3 = swarmlib.Drone(cf_names[2], obstacles)
centroid = swarmlib.Centroid(obstacles)
centroid2 = swarmlib.Centroid(obstacles, 'centroid2')
""" trajectory generation """ # TODO
# setpoints = []
# for obstacle in obstacles:
# setpoints.append(obstacle.pose)
# print setpoints
if direction == 'back':
xs, ys, zs = np.array([ drone1.pose[0]-R_swarm, drone1.pose[1], TakeoffHeight]) if toFly else np.array([obstacles[-1].position()[0]+0.5-R_swarm, obstacles[-1].position()[1], TakeoffHeight])
xg, yg, zg = np.array([obstacles[0].pose[0]-0.5-R_swarm, obstacles[0].pose[1], TakeoffHeight])
return group_numbers, G_global, G_parts, boundary
def hullpts(pts):
ch = ConvexHull(pts)
hull_indices = ch.vertices
hull_pts = pts[hull_indices, :]
return hull_pts
if __name__ == '__main__':
rospy.init_node('find_borders', anonymous=True)
# Objects init
obstacle = np.array([])
for i in range(len(obstacle_names)):
obstacle = np.append(obstacle, swarmlib.Obstacle(obstacle_names[i], i))
# INITIALIZATION
visualize = 1
legend_list = []
N = 9
R = 0.15
points = np.random.rand(N, 2)
group_numbers, G_global, G_parts, boundary = close_pts(points, R)
print "Obstacles' numbers in groups:\n" + str(group_numbers)
if visualize:
# VISUALIZATION
fig, (ax1, ax2, ax3) = plt.subplots(ncols=3, figsize=(20, 10))
ax1 = plt.subplot(131)
ax1.set_aspect('equal')