def main():
SLEEP = 3
signal.signal(signal.SIGINT, signal_handler)
global cf1
global cf2
rospy.init_node('test_2Drone_movement')
cf1 = crazyflie.Crazyflie(DRONE1_NAME, DRONE1_NAME)
cf2 = crazyflie.Crazyflie(DRONE2_NAME, DRONE2_NAME)
cf1.setParam("commander/enHighLevel", 1)
cf2.setParam("commander/enHighLevel", 1) #Check later
print("starting")
traj1 = uav_trajectory.Trajectory()
traj1.loadcsv("takeoff.csv")
time.sleep(traj1.duration * 2.0)
print("Drone crazyflie2 should take off")
cf1.uploadTrajectory(0, 0, traj1)
cf1.startTrajectory(0, timescale=1.0)
time.sleep(traj1.duration * 2.0)
time.sleep(SLEEP)
print("Drone crazyflie3 should take off")
cf2.uploadTrajectory(0, 0, traj1)
cf2.startTrajectory(0, timescale=1.0)
time.sleep(SLEEP)
#movement after take off
print("Drones should now go to 1st positions")
#each drone moves to a different edge
cf1.goTo(goal = [-0.7, 0, 0.5], yaw=0.2, duration = 2.0, relative = False)
cf2.goTo(goal = [1, 0, 0.5], yaw=0.2, duration = 2.0, relative = False)
time.sleep(SLEEP)
#Each drone move a bit to the center
cf1.goTo(goal = [-0.5, 0, 0.5], yaw=0.2, duration = 2.0, relative = False)
cf2.goTo(goal = [0.8, 0, 0.5], yaw=0.2, duration = 2.0, relative = False)
time.sleep(SLEEP)
#Each drone goes to a diffrent side
cf1.goTo(goal = [-0.5, 0.4, 0.5], yaw=0.2, duration = 2.0, relative = False)
cf2.goTo(goal = [0.8, -0.4, 0.5], yaw=0.2, duration = 2.0, relative = False)
time.sleep(SLEEP)
#Each drone goes to a diffrent side
cf1.goTo(goal = [-0.5, -0.4, 0.5], yaw=0.2, duration = 2.0, relative = False)
cf2.goTo(goal = [0.8, 0.4, 0.5], yaw=0.2, duration = 2.0, relative = False)
time.sleep(SLEEP)
#Each drone goes a bit to the center
cf1.goTo(goal = [-0.3, -0.4, 0.5], yaw=0.2, duration = 2.0, relative = False)
cf2.goTo(goal = [0.6, 0.4, 0.5], yaw=0.2, duration = 2.0, relative = False)
time.sleep(SLEEP)
cf1.land(targetHeight = 0.0, duration = 2.0)
cf2.land(targetHeight = 0.0, duration = 2.0)
time.sleep(SLEEP)
#stop drones
cf1.stop()
cf2.stop()
def cf_init(cf_name):
cf = crazyflie.Crazyflie(cf_name, '/vicon/' + cf_name + '/' + cf_name)
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
return cf
def reset(self):
""" Returns the drone to a starting postion to begin a new training episode.
Returns:
ndarray (dtype=float, ndim=1): Array containing the current state observation.
"""
self.steps = 0
# Connect to Crazyflie and enable high-level control
self.cf = crazyflie.Crazyflie("cf" + str(self.cf_id),
"/cf" + str(self.cf_id))
self.cf.setParam("commander/enHighLevel", 1)
reset_positions = self.random_position(-4, 5, 1, 10, 1)
print('Start Reset')
self.cf.takeoff(targetHeight=reset_positions[0][2], duration=4)
# time.sleep(4)
# check if need to get individual values from np array
self.cf.goTo(goal=[
reset_positions[0][0], reset_positions[0][1], reset_positions[0][2]
],
yaw=0.0,
duration=4)
# time.sleep(1)
# print('End Reset')
observation = self.get_observation()
return observation
def main():
signal.signal(signal.SIGINT, signal_handler)
global cf
rospy.init_node('test_high_level')
cf = crazyflie.Crazyflie(DRONE_NAME, DRONE_NAME)
cf.setParam("commander/enHighLevel", 1)
print("starting")
traj1 = uav_trajectory.Trajectory()
traj1.loadcsv("takeoff.csv")
traj2 = uav_trajectory.Trajectory()
traj2.loadcsv("figure8.csv")
print(traj1.duration)
cf.uploadTrajectory(0, 0, traj1)
cf.uploadTrajectory(1, len(traj1.polynomials), traj2)
cf.startTrajectory(0, timescale=1.0)
time.sleep(traj1.duration * 2.0)
while 1:
battery_listener() # print battey level
time.sleep(1)
cf.startTrajectory(1, timescale=2.0)
time.sleep(traj2.duration * 2.0)
cf.startTrajectory(0, timescale=1.0, reverse=True)
time.sleep(traj1.duration * 1.0)
cf.stop()
def run(drones):
print "init node"
rospy.init_node('figure8')
print "starting"
listener = tf.TransformListener()
cfs = []
for drone_name in drones:
cf = crazyflie.Crazyflie(drone_name, listener)
cfs.append(cf)
cf.setParam("commander/enHighLevel", 1)
print("starting")
traj1 = uav_trajectory.Trajectory()
traj1.loadcsv("./csv/takeoff.csv")
traj2 = uav_trajectory.Trajectory()
traj2.loadcsv("./csv/figure8.csv")
print(traj1.duration)
for cf in cfs:
cf.uploadTrajectory(0, 0, traj1)
cf.uploadTrajectory(1, len(traj1.polynomials), traj2)
cf.startTrajectory(0, timescale=1.0)
time.sleep(traj1.duration * 2.0)
while True:
for cf in cfs:
cf.startTrajectory(1, timescale=2.0)
time.sleep(traj2.duration * 2.0)
for cf in cfs:
cf.startTrajectory(0, timescale=1.0, reverse=True)
time.sleep(traj1.duration * 1.0)
for cf in cfs:
cf.land(targetHeight=0.0, duration=2.0)
def reset(self):
self.unpauseSim()
self.steps = 0
self.steps_since_avoided = np.zeros(self.n_obstacles)
reset_positions = self.random_position(-4, 5, 1, 10,
self.n_obstacles + 1)
print('Start Reset')
# Connect to Crazyflie and enable high-level control
for i in range(self.n_obstacles + 1):
self.cfs[i] = crazyflie.Crazyflie('cf' + str(i + 1),
'/cf' + str(i + 1))
self.cfs[i].setParam("commander/enHighLevel", 1)
time.sleep(2)
for i in range(self.n_obstacles + 1):
self.cfs[i].takeoff(targetHeight=reset_positions[i][2], duration=4)
time.sleep(4)
for i in range(self.n_obstacles + 1):
self.cfs[i].goTo(goal=[
reset_positions[i][0], reset_positions[i][1],
reset_positions[i][2]
],
yaw=0.0,
duration=4)
time.sleep(4)
print('End Reset')
self.pauseSim()
return self.get_observation()
def __init__(self, use_controller, joy_topic, prefix, tf):
self.cf = crazyflie.Crazyflie(prefix, tf)
self.cf.setParam("commander/enHighLevel", 1)
self.cf.setParam("stabilizer/estimator", 2) # Use EKF
self.cf.setParam("stabilizer/controller",
2) # Use mellinger controller
# reset kalman
self.cf.setParam("kalman/resetEstimation", 1)
rospy.loginfo("waiting for emergency service")
rospy.wait_for_service('emergency')
rospy.loginfo("found emergency service")
self._emergency = rospy.ServiceProxy('emergency', Empty)
if use_controller:
rospy.loginfo("waiting for land service")
rospy.wait_for_service('land')
rospy.loginfo("found land service")
self.cf.landService
rospy.loginfo("waiting for takeoff service")
rospy.wait_for_service('takeoff')
rospy.loginfo("found takeoff service")
self.cf.takeoffService
else:
self.cf.landService = None
self.cf.takeoffService = None
# subscribe to the joystick at the end to make sure that all required
# services were found
self._buttons = None
rospy.Subscriber(joy_topic, Joy, self._joyChanged)
def takeoff(cf_name):
cf = crazyflie.Crazyflie(cf_name, '/vicon/' + cf_name + '/' + cf_name)
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
for _ in range(3):
print "takeoff.. ", cf.prefix
cf.takeoff(targetHeight=TAKEOFFHEIGHT, duration=TAKEOFFTIME)
time.sleep(TAKEOFFTIME + 2.0)
def test(cf_name):
cf1 = crazyflie.Crazyflie(cf_name, '/vicon/'+cf_name+'/'+cf_name)
cf1.setParam("commander/enHighLevel", 1)
cf1.setParam("stabilizer/estimator", 2) # Use EKF
cf1.setParam("stabilizer/controller", 2) # Use Mellinger controller
cf1.takeoff(targetHeight = TakeoffHeight, duration = TakeoffTime)
time.sleep(TakeoffTime+1)
cf1.land(targetHeight = 0.0, duration = TakeoffTime+3)
time.sleep(3.0)
def __init__(
self, name, world_size_x, world_size_y, flight_height
): # world_size_x = WORLD_RANGE["X"][0] # world_size_y = WORLD_RANGE["Y"][0]
self._name = name
self._status = "Landed"
self._move_speed = 0.2
self._step_size = 0.2
self._world_size_x = world_size_x
self._world_size_y = world_size_y
self._flight_height = flight_height
self._listener = tf.TransformListener()
self._cf = crazyflie.Crazyflie(name, self._listener)
self._cf.setParam("commander/enHighLevel", 1)
self._cf.monitorBattery()
def main():
signal.signal(signal.SIGINT, signal_handler)
global CF_LIST
test = sys.argv[1]
valid_tests = os.listdir('./tests')
if sys.argv[1] + ".py" not in valid_tests:
print("Please enter a valid test name")
return 0
arguments = renameArguments(sys.argv[2:])
for drone_name in arguments:
print "Create object for drone_name".format(drone_name)
cf = crazyflie.Crazyflie(drone_name, drone_name)
CF_LIST.append(cf)
globals()[test].run(arguments)
def run(drones):
print "init node"
rospy.init_node('land')
print "starting"
listener = tf.TransformListener()
cfs = []
for drone_name in drones:
try:
cf = crazyflie.Crazyflie(drone_name, drone_name)
cfs.append(cf)
cf.setParam("commander/enHighLevel", 1)
cf.land(targetHeight=0.0, duration=2.0)
print "Land {}".format(drone_name)
except:
print "Skip {}".format(drone_name)
def __init__(self, prefix, initialZ=0.35):
self.cf = crazyflie.Crazyflie("/" + prefix, "world")
self.initialZ = initialZ
self.ranges_sub = rospy.Subscriber('/' + prefix + '/log_ranges',
GenericLogData, self.get_ranges)
self.Cj_injection_sub = rospy.Subscriber('/' + prefix + '/Cj_injcetor',
PoseStamped, self.Cj_injector)
self.keyboard_listener = rospy.Subscriber("/cmd_vel", Twist,
self.twist_callback)
self.tfBuffer = tf2_ros.Buffer(
) # initialize tf buffer for transform lookupupdate_params
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.cj_injection_flag = False
self.Cj_injection = [None, None, None, None, None]
self.ranges = [None, None, None, None, None]
self.keyboard_flag = False
self.keyboard = [None, None, None, None, None]
def run(drones):
print "init node"
rospy.init_node('fast_move')
print "starting"
cfs = []
for drone_name in drones:
cf = crazyflie.Crazyflie(drone_name, drone_name)
cfs.append(cf)
cf.setParam("commander/enHighLevel", 1)
cf.takeoff(targetHeight=0.5, duration=2.5)
time.sleep(2.5)
xy_pos = [[0,0], [0.3,0], [0.6,0], [0.3,0], [0,0], [-0.5,0]]
for cf in cfs:
for pos in xy_pos:
print "Got to {}".format(pos)
cf.goTo(goal=[pos[0],pos[1],0.5], yaw=0.0, duration=1, relative=False)
time.sleep(0.2)
cf.land(targetHeight=0.0, duration=2.0)
def run(drones):
print "init node"
rospy.init_node('hover')
print "starting"
listener = tf.TransformListener()
cfs = []
for drone_name in drones:
cf = crazyflie.Crazyflie(drone_name, listener)
cfs.append(cf)
cf.setParam("commander/enHighLevel", 1)
(real_x, real_y,
real_z), rot = listener.lookupTransform("/world",
"/{}".format(drone_name),
rospy.Time(0))
print "Takeoff from ({:.2f} , {:.2f} , {:.2f})".format(
real_x, real_y, real_z)
cf.takeoff(targetHeight=0.5, duration=2.5)
time.sleep(2.5)
time.sleep(120)
for cf in cfs:
cf.land(targetHeight=0.0, duration=2.0)
def run(drones):
print "init node"
rospy.init_node('land')
print "starting"
listener = tf.TransformListener()
cfs = []
leds = []
for drone_name in drones:
if drone_name[:9] == "crazyflie":
cf = crazyflie.Crazyflie(drone_name, listener)
cfs.append(cf)
cf.setParam("commander/enHighLevel", 1)
elif drone_name[:3] == "led":
ld = led.Led(drone_name, 0, 0)
leds.append(ld)
while True:
pos_list = []
for cf in cfs:
pos_list.append("{} is at {}".format(cf.prefix, cf.position()))
for ld in leds:
pos_list.append("{} is at {}".format(ld.prefix, ld.getPosition()))
print "\n".join(pos_list)
def run(drones):
print "init node"
rospy.init_node('square_relative')
print "starting"
cfs = []
listener = tf.TransformListener()
for drone_name in drones:
cf = crazyflie.Crazyflie(drone_name, listener)
cfs.append(cf)
cf.setParam("commander/enHighLevel", 1)
cf.takeoff(targetHeight=0.5, duration=2.5)
time.sleep(2.5)
for cf, drone_name in zip(cfs, drones):
move_relative(cf, OFFSET, 0)
for cf, drone_name in zip(cfs, drones):
move_relative(cf, 0, OFFSET)
for cf, drone_name in zip(cfs, drones):
move_relative(cf, -OFFSET, 0)
for cf, drone_name in zip(cfs, drones):
move_relative(cf, 0, -OFFSET)
for cf, drone_name in zip(cfs, drones):
cf.land(targetHeight=0.0, duration=2.0)
time.sleep(1.5)
cf.stop()
rospy.loginfo('*******keyboard input exception')
rospy.loginfo(e)
if __name__ == '__main__':
rospy.init_node('keyboard_controller')
prefix = rospy.get_param("~tf_prefix")
rospy.Subscriber('/' + prefix + '/log_ranges', GenericLogData, get_ranges)
pub_hover = rospy.Publisher(prefix + "/cmd_hover", Hover,
queue_size=1) #hover message publisher
msg = Hover()
msg.header.seq = 0
cf = crazyflie.Crazyflie("/" + prefix, "world")
rospy.wait_for_service("/" + prefix + '/update_params')
rospy.loginfo("found update_params service")
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("ctrlMel/kp_z", 0.8) # reduce z wobble - default 1.25
# cf.setParam("ctrlMel/ki_z", 0.06) #reduce z wobble - default 0.05
# cf.setParam("ctrlMel/kd_z", 0.2) #reduce z wobble - default 0.4
# cf.setParam("ctrlMel/i_range_z", 0.2) #reduce z wobble
# reset kalman
cf.setParam("kalman/initialX", 0)
cf.setParam("kalman/initialY", 0)
"""
@author: Daniel Duberg ([email protected])
"""
import rospy
import crazyflie
import time
import math
if __name__ == '__main__':
rospy.init_node('test_high_level')
cf_prefix = rospy.get_param("~cf_prefix", "cf1")
tf_prefix = rospy.get_param("~tf_prefix", "/cf1")
should_takeoff = rospy.get_param('~takeoff', False)
takeoff_height = rospy.get_param('~takeoff_height', 0.2)
takeoff_duration = rospy.get_param("~takeoff_duration", 2.0)
sleep_sec_before_takeoff = rospy.get_param(
'~sleep_sec_before_takeoff', 3.0)
cf = crazyflie.Crazyflie(cf_prefix, tf_prefix)
cf.setParam("commander/enHighLevel", 1)
if should_takeoff:
print("Takeoff in {} seconds".format(sleep_sec_before_takeoff))
time.sleep(sleep_sec_before_takeoff)
cf.takeoff(targetHeight=takeoff_height, duration=takeoff_duration)
(cf_pose.pose.position.x, cf_pose.pose.position.y,
cf_pose.pose.position.z),
(cf_pose.pose.orientation.x, cf_pose.pose.orientation.y,
cf_pose.pose.orientation.z, cf_pose.pose.orientation.w),
rospy.Time.now(), "cf", "base")
if __name__ == '__main__':
firstSLAMPose = True
gotCFPose = False
gotCFPose_beforeSLAM = False
rospy.init_node('crazybal_slam_demo')
rospy.loginfo("started commander in" + os.getcwd())
cf = crazyflie.Crazyflie("/crazyflie", "/cf")
rospy.loginfo("started cf")
# update all relevant parametes before flight?
orbslam_pose_topic = rospy.get_param("~orbslam_pose_topic",
"crazyflie/orb_slam2_mono/pose")
crazy_flie_pose_topic = rospy.get_param("~crazy_flie_pose_topic",
"/crazyflie/pose")
tf_br = tf.TransformBroadcaster()
tf_listener = TransformListener()
# SUBSCRIBERS
rospy.Subscriber(orbslam_pose_topic, PoseStamped, new_slam_pose)
rospy.Subscriber(crazy_flie_pose_topic, PoseStamped, new_cf_pose)
#!/usr/bin/env python
import rospy
import crazyflie
if __name__ == '__main__':
rospy.init_node('test_two_high_level')
cfR = crazyflie.Crazyflie("crazyflieR", "crazyflieR")
cfR.setParam("commander/enHighLevel", 1)
cfG = crazyflie.Crazyflie("crazyflieG", "crazyflieG")
cfG.setParam("commander/enHighLevel", 1)
cfR.stop()
cfG.stop()
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)
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
cf_list.append(cf)
for t in range(3):
print "takeoff.. ", cf.prefix
for cf in cf_list:
cf.takeoff(targetHeight = TAKEOFFHEIGHT, duration = TAKEOFFTIME)
time.sleep(TAKEOFFTIME+2.0)
# go to approximate human location
# for t in range(3):
# for cf in cf_list:
# cf.goTo(goal = [0.5, 0.5, 0], yaw=0.0, duration=3.0, relative = True)
# time.sleep(3.5)
cf.setParam("motion/disable", 1)
rospy.loginfo("Switch Flow Deck OFF")
return -1
else:
# Void Joystick command
return -1
if __name__ == '__main__':
rospy.init_node("joystick_controller")
rospy.loginfo("Initializing Joystick Controller...")
config = load_params()
rospy.Subscriber("/joy", Joy, joy_cb)
# Connecting to Crazyflie
cf = crazyflie.Crazyflie("/crazyflie", "crazyflie/base_link")
cf.setParam("commander/enHighLevel", 1)
cf.setParam("locSrv/useExtPos", 0)
cf.reset_ekf()
print(config["target_durations"])
print(config["target_steps"])
print(cf.target)
# setTargetSteps(config['target_steps'], config['target_durations'])
rospy.loginfo("Finish Setting up Crazyflie. Ready to take off...")
state = DroneState.Idle
while (not rospy.is_shutdown()):
if last_joy_msg != None:
new_state = DroneState.joy_nextstate(state, last_joy_msg)
if new_state == DroneState.Taking_Off:
current_action_duration = config["target_height"] / config[
import sys
if __name__ == '__main__':
if len(sys.argv) != 3:
print "Wrong number of arguments !"
print "Example : python max_uav_control.py [nbQuads] [tfPrefix]"
sys.exit()
nbQuads = int(sys.argv[1])
cfPrefix = str(sys.argv[2])
rospy.init_node('test_high_level_multi_cf')
listcf = list()
for iter in range(nbQuads):
listcf.append(
crazyflie.Crazyflie(cfPrefix + str(iter + 1),
"/" + cfPrefix + str(iter + 1)))
for cf_i in listcf:
cf_i.setParam("commander/enHighLevel", 1)
time.sleep(1.0)
for cf_i in listcf:
cf_i.takeoff(targetHeight=0.5, duration=10.0)
time.sleep(7.0)
for cf_i in listcf:
cf_i.goTo(goal=[0.5, 0.0, 0.0], yaw=0.2, duration=2.0, relative=True)
time.sleep(7.0)
for cf_i in listcf:
#!/usr/bin/env python
import rospy
import crazyflie
import time
import uav_trajectory
if __name__ == '__main__':
rospy.init_node('test_high_level')
cf = crazyflie.Crazyflie("crazyflie", "/vicon/crazyflie/crazyflie")
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
# reset kalman
cf.setParam("kalman/resetEstimation", 1)
cf.takeoff(targetHeight=0.5, duration=2.0)
time.sleep(3.0)
cf.goTo(goal=[0.5, 0.0, 0.0], yaw=0.2, duration=2.0, relative=True)
time.sleep(3.0)
cf.land(targetHeight=0.0, duration=2.0)
# traj1 = uav_trajectory.Trajectory()
# traj1.loadcsv("takeoff.csv")
# traj2 = uav_trajectory.Trajectory()
def circle_trajectory(freq, duration,r):
x = list()
y = list()
t = list()
for i in range(freq):
t0 = 2 * math.pi * (1.0/(freq-1)) * duration * i
x.append(r * math.cos(t0/duration))
y.append(r * math.sin(t0/duration))
t.append((1.0/(freq-1)) * duration * i)
return (t,x,y)
if __name__ == '__main__':
rospy.init_node('test_high_level')
cf = crazyflie.Crazyflie("cf1", "/cf1")
cf.setParam("commander/enHighLevel", 1)
cf.takeoff(targetHeight = 3.0, duration = 1.0)
time.sleep(5.0)
# cf.goTo(goal = [9.0, 9.0, 9.0], yaw=0.0, duration = 0.05, relative = False)
# time.sleep(4)
# cf.land(targetHeight = 0.0 , duration = 4.0)
# time.sleep(4)
(t , x , y) = circle_trajectory(50 , 10 , 0.8)
for i in range(len(x)):
cf.goTo(goal = [x[i], y[i], 1.5], yaw=0.0, duration = 10.0/50.0, relative = False)
import tf
# from crazyflie_driver.msg import Hover
from std_msgs.msg import Empty
from crazyflie_driver.srv import UpdateParams
from crazyflie_driver.msg import GenericLogData
from threading import Thread
import tty, termios
import sys
if __name__ == '__main__':
rospy.init_node('test_high_level')
# rospy.Subscriber('/cf1/log_ranges', GenericLogData, get_ranges)
prefix = '/cf1'
cf = crazyflie.Crazyflie("/cf1", "world")
rospy.wait_for_service(prefix + '/update_params')
rospy.loginfo("found update_params service")
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("ctrlMel/kp_z", 1.0) # reduce z wobble - default 1.25
# cf.setParam("ctrlMel/ki_z", 0.06) # reduce z wobble - default 0.05
# cf.setParam("ctrlMel/kd_z", 0.2) # reduce z wobble - default 0.4
## reset kalman
cf.setParam("kalman/initialX", 0)
cf.setParam("kalman/initialY", 0)
cf.setParam("kalman/initialZ", 0)
cf.setParam("kalman/resetEstimation", 1)
#!/usr/bin/env python
import rospy
import crazyflie
import time
import uav_trajectory
if __name__ == '__main__':
rospy.init_node('test_high_level_multi_cf')
cf1 = crazyflie.Crazyflie("cf1", "/cf1")
cf2 = crazyflie.Crazyflie("cf2", "/cf2")
cf1.setParam("commander/enHighLevel", 1)
cf2.setParam("commander/enHighLevel", 1)
cf1.takeoff(targetHeight=0.5, duration=10.0)
cf2.takeoff(targetHeight=0.5, duration=10.0)
time.sleep(10.0)
# cf.land(targetHeight = 0.0 , duration = 4.0)
# time.sleep(20.0)
# rospy.spin()
cf1.goTo(goal=[0.5, 0.0, 0.0], yaw=0.2, duration=2.0, relative=True)
cf2.goTo(goal=[0.5, 0.0, 0.0], yaw=0.2, duration=2.0, relative=True)
time.sleep(10.0)
cf1.land(targetHeight=0.0, duration=2.0)
cf2.land(targetHeight=0.0, duration=2.0)
time.sleep(10.0)
def onNewTransform(pose):
global cfR
global counter
global ref_rate
global ref_samples
green_pose = [pose.pose.position.x , pose.pose.position.y , pose.pose.position.z]
print(green_pose)
counter+= 1
if(counter == ref_samples):
cfR.land(targetHeight = 0.0, duration = 2.0)
rospy.sleep(3.0)
cfR.stop()
elif(counter < ref_samples):
cfR.goTo(goal = green_pose, yaw=0.0, duration = ref_rate, relative = False)
rospy.sleep(ref_rate)
if __name__ == '__main__':
rospy.init_node('test_high_level')
cfR = crazyflie.Crazyflie("crazyflieR", "crazyflieR")
cfR.setParam("commander/enHighLevel", 1)
counter = 0
ref_rate = 1.0
ref_samples = 30
cfR.takeoff(targetHeight = 0.1 ,duration = 2.0)
rospy.sleep(3.0)
rospy.Subscriber("/ball_poseG", PoseStamped, onNewTransform , queue_size = 1)
rospy.spin()
# obstacles
for pose in real_obstacles:
circle = plt.Circle(pose, R_obstacles, color='yellow')
ax.add_artist(circle)
for pose in fake_obstacles:
circle = plt.Circle(pose, R_obstacles, color='blue')
ax.add_artist(circle)
PATH = '/home/ruslan/Desktop/'
# savedata(route, fig, PATH)
if visualize:
plt.show()
""" Flight: takeoff -> trajectory following -> landing """
if toFly:
print("takeoff")
cf1 = crazyflie.Crazyflie(cf_names[2],
'/vicon/' + cf_names[2] + '/' + cf_names[2])
cf1.setParam("commander/enHighLevel", 1)
cf1.setParam("stabilizer/estimator", 2) # Use EKF
cf1.setParam("stabilizer/controller", 2) # Use Mellinger controller
cf1.takeoff(targetHeight=TakeoffHeight, duration=TakeoffTime)
time.sleep(TakeoffTime + 1)
cf1.land(targetHeight=0.0, duration=2.0)
time.sleep(3.0)
# rate = rospy.Rate(40)
# msg = Position()
# msg.header.seq = 0
# msg.header.stamp = rospy.Time.now()
# msg.header.frame_id = "/world"
