class AviaryWrapper(Node):
#### Initialize the node ###################################
def __init__(self):
super().__init__('aviary_wrapper')
self.step_cb_count = 0
self.get_action_cb_count = 0
timer_freq_hz = 240
timer_period_sec = 1 / timer_freq_hz
#### Create the CtrlAviary environment wrapped by the node #
self.env = CtrlAviary(drone_model=DroneModel.CF2X,
num_drones=1,
neighbourhood_radius=np.inf,
initial_xyzs=None,
initial_rpys=None,
physics=Physics.PYB,
freq=timer_freq_hz,
aggregate_phy_steps=1,
gui=True,
record=False,
obstacles=False,
user_debug_gui=True)
#### Initialize an action with the RPMs at hover ###########
self.action = np.ones(4) * self.env.HOVER_RPM
#### Declare publishing on 'obs' and create a timer to call
#### action_callback every timer_period_sec ################
self.publisher_ = self.create_publisher(Float32MultiArray, 'obs', 1)
self.timer = self.create_timer(timer_period_sec, self.step_callback)
#### Subscribe to topic 'action' ###########################
self.action_subscription = self.create_subscription(
Float32MultiArray, 'action', self.get_action_callback, 1)
self.action_subscription # prevent unused variable warning
#### Step the env and publish drone0's state on topic 'obs'
def step_callback(self):
self.step_cb_count += 1
obs, reward, done, info = self.env.step({"0": self.action})
msg = Float32MultiArray()
msg.data = obs["0"]["state"].tolist()
self.publisher_.publish(msg)
if self.step_cb_count % 10 == 0:
self.get_logger().info('Publishing obs: "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f"' \
% (msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4],
msg.data[5], msg.data[6], msg.data[7], msg.data[8], msg.data[9],
msg.data[10], msg.data[11], msg.data[12], msg.data[13], msg.data[14],
msg.data[15], msg.data[16], msg.data[17], msg.data[18], msg.data[19]
)
)
#### Read the action to apply to the env from topic 'action'
def get_action_callback(self, msg):
self.get_action_cb_count += 1
self.action = np.array(
[msg.data[0], msg.data[1], msg.data[2], msg.data[3]])
if self.get_action_cb_count % 10 == 0:
self.get_logger().info(
'I received action: "%f" "%f" "%f" "%f"' %
(msg.data[0], msg.data[1], msg.data[2], msg.data[3]))
num_drones=1)
#### Initialize the controller #############################
ctrl = DSLPIDControl(drone_model=DroneModel.CF2X)
#### Run the simulation ####################################
CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz))
action = {"0": np.array([0, 0, 0, 0])}
START = time.time()
for i in range(0, int(ARGS.duration_sec * env.SIM_FREQ), AGGR_PHY_STEPS):
#### Make it rain rubber ducks #############################
# if i/env.SIM_FREQ>5 and i%10==0 and i/env.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [0+random.gauss(0, 0.3),-0.5+random.gauss(0, 0.3),3], p.getQuaternionFromEuler([random.randint(0,360),random.randint(0,360),random.randint(0,360)]), physicsClientId=PYB_CLIENT)
#### Step the simulation ###################################
obs, reward, done, info = env.step(action)
#### Compute control at the desired frequency ##############
if i % CTRL_EVERY_N_STEPS == 0:
#### Compute control for the current way point #############
action["0"], _, _ = ctrl.computeControlFromState(
control_timestep=CTRL_EVERY_N_STEPS * env.TIMESTEP,
state=obs["0"]["state"],
target_pos=TARGET_POS[wp_counter, :],
)
#### Go to the next way point and loop #####################
wp_counter = wp_counter + 1 if wp_counter < (NUM_WP - 1) else 0
#### Log the simulation ####################################
#### Derive the target trajectory to obtain target velocities and accelerations
TARGET_VELOCITY[1:, :] = (TARGET_POSITION[1:, :] -
TARGET_POSITION[0:-1, :]) / ENV.SIM_FREQ
TARGET_ACCELERATION[1:, :] = (TARGET_VELOCITY[1:, :] -
TARGET_VELOCITY[0:-1, :]) / ENV.SIM_FREQ
#### Run the simulation ####################################
START = time.time()
for i in range(0, DURATION * ENV.SIM_FREQ):
### Secret control performance booster #####################
# if i/ENV.SIM_FREQ>3 and i%30==0 and i/ENV.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [random.gauss(0, 0.3), random.gauss(0, 0.3), 3], p.getQuaternionFromEuler([random.randint(0, 360),random.randint(0, 360),random.randint(0, 360)]), physicsClientId=PYB_CLIENT)
#### Step the simulation ###################################
OBS, _, _, _ = ENV.step(ACTION)
#### Compute control for drone 0 ###########################
STATE = OBS["0"]["state"]
ACTION["0"] = CTRL_0.compute_control(
current_position=STATE[0:3],
current_velocity=STATE[10:13],
current_rpy=STATE[7:10],
target_position=TARGET_POSITION[i, :],
target_velocity=TARGET_VELOCITY[i, :],
target_acceleration=TARGET_ACCELERATION[i, :])
#### Log drone 0 ###########################################
LOGGER.log(drone=0, timestamp=i / ENV.SIM_FREQ, state=STATE)
#### Compute control for drone 1 ###########################
STATE = OBS["1"]["state"]
class start_drone():
def __init__(self):
parser = argparse.ArgumentParser(
description=
'Helix flight script using CtrlAviary or VisionAviary and DSLPIDControl'
)
parser.add_argument('--drone',
default="cf2x",
type=DroneModel,
help='Drone model (default: CF2X)',
metavar='',
choices=DroneModel)
parser.add_argument('--num_drones',
default=1,
type=int,
help='Number of drones (default: 3)',
metavar='')
parser.add_argument('--physics',
default="pyb",
type=Physics,
help='Physics updates (default: PYB)',
metavar='',
choices=Physics)
parser.add_argument(
'--vision',
default=False,
type=str2bool,
help='Whether to use VisionAviary (default: False)',
metavar='')
parser.add_argument('--gui',
default=True,
type=str2bool,
help='Whether to use PyBullet GUI (default: True)',
metavar='')
parser.add_argument('--record_video',
default=False,
type=str2bool,
help='Whether to record a video (default: False)',
metavar='')
parser.add_argument(
'--plot',
default=False,
type=str2bool,
help='Whether to plot the simulation results (default: True)',
metavar='')
parser.add_argument(
'--user_debug_gui',
default=False,
type=str2bool,
help=
'Whether to add debug lines and parameters to the GUI (default: False)',
metavar='')
parser.add_argument(
'--aggregate',
default=False,
type=str2bool,
help='Whether to aggregate physics steps (default: False)',
metavar='')
parser.add_argument(
'--obstacles',
default=False,
type=str2bool,
help='Whether to add obstacles to the environment (default: True)',
metavar='')
parser.add_argument('--simulation_freq_hz',
default=240,
type=int,
help='Simulation frequency in Hz (default: 240)',
metavar='')
parser.add_argument('--control_freq_hz',
default=48,
type=int,
help='Control frequency in Hz (default: 48)',
metavar='')
parser.add_argument(
'--duration_sec',
default=1000,
type=int,
help='Duration of the simulation in seconds (default: 5)',
metavar='')
self.ARGS = parser.parse_args()
H = .1
H_STEP = .05
R = .3
INIT_XYZS = np.array([[
R * np.cos((i / 6) * 2 * np.pi + np.pi / 2),
R * np.sin((i / 6) * 2 * np.pi + np.pi / 2) - R, H + i * H_STEP
] for i in range(self.ARGS.num_drones)])
INIT_RPYS = np.array([[0, 0, i * (np.pi / 2) / self.ARGS.num_drones]
for i in range(self.ARGS.num_drones)])
AGGR_PHY_STEPS = int(
self.ARGS.simulation_freq_hz /
self.ARGS.control_freq_hz) if self.ARGS.aggregate else 1
if self.ARGS.vision:
self.env = VisionAviary(drone_model=self.ARGS.drone,
num_drones=self.ARGS.num_drones,
initial_xyzs=INIT_XYZS,
initial_rpys=INIT_RPYS,
physics=self.ARGS.physics,
neighbourhood_radius=10,
freq=self.ARGS.simulation_freq_hz,
aggregate_phy_steps=AGGR_PHY_STEPS,
gui=self.ARGS.gui,
record=self.ARGS.record_video,
obstacles=self.ARGS.obstacles)
else:
self.env = CtrlAviary(drone_model=self.ARGS.drone,
num_drones=self.ARGS.num_drones,
initial_xyzs=INIT_XYZS,
initial_rpys=INIT_RPYS,
physics=self.ARGS.physics,
neighbourhood_radius=10,
freq=self.ARGS.simulation_freq_hz,
aggregate_phy_steps=AGGR_PHY_STEPS,
gui=self.ARGS.gui,
record=self.ARGS.record_video,
obstacles=self.ARGS.obstacles,
user_debug_gui=self.ARGS.user_debug_gui)
PYB_CLIENT = self.env.getPyBulletClient()
if self.ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = [
DSLPIDControl(self.env) for i in range(self.ARGS.num_drones)
]
elif self.ARGS.drone in [DroneModel.HB]:
self.ctrl = [
SimplePIDControl(self.env) for i in range(self.ARGS.num_drones)
]
self.CTRL_EVERY_N_STEPS = int(
np.floor(self.env.SIM_FREQ / self.ARGS.control_freq_hz))
self.action = {
str(i): np.array([0, 0, 0, 0])
for i in range(self.ARGS.num_drones)
}
self.START = time.time()
def move(self, iterator, target_pos, target_ori):
obs, reward, done, info = self.env.step(self.action)
#### Compute control at the desired frequency ##############
if iterator % self.CTRL_EVERY_N_STEPS == 0:
#### Compute control for the current way point #############
for j in range(self.ARGS.num_drones):
self.action[str(
j)], _, _ = self.ctrl[j].computeControlFromState(
state=obs[str(j)]["state"],
target_pos=np.array(target_pos),
target_rpy=np.array(target_ori))
#### Sync the simulation ###################################
if self.ARGS.gui:
sync(iterator, self.START, self.env.TIMESTEP)
