Fix deploy script

lsy_drone_racing/command.py

@@ -69,6 +69,8 @@ def apply_command(cf: Crazyflie, command_type: Command, args: Any):
         args: Additional arguments as potentially required by `command_type`.
     """
     if command_type == Command.FULLSTATE:
+        # Sim version takes an additional 'ep_time' args that needs to be removed when deployed
+        args = args[:5]
         cf.cmdFullState(*args)
     elif command_type == Command.TAKEOFF:
         cf.takeoff(*args)

lsy_drone_racing/vicon.py

@@ -25,7 +25,10 @@ def __init__(self, track_names: list[str] = []):
             track_names: The names of any additional objects besides the drone to track.
         """
         assert Master("/rosnode").is_online(), "ROS is not running. Please run hover.launch first!"
-        rospy.init_node("playback_node")
+        try:
+            rospy.init_node("playback_node")
+        except rospy.exceptions.ROSException:
+            ...  # ROS node is already running which is fine for us
         config_path = get_ros_package_path("crazyswarm") / "launch/crazyflies.yaml"
         assert config_path.exists(), "Crazyfly config file missing!"
         with open(config_path, "r") as f:

scripts/deploy.py

@@ -9,24 +9,25 @@
 
 from __future__ import annotations
 
+import logging
 import pickle
 import time
 from pathlib import Path
-from typing import Optional
 
 import fire
 import numpy as np
-import rospkg
 import rospy
 import yaml
-from pycrazyswarm import Crazyswarm
 from safe_control_gym.utils.configuration import ConfigFactory
 from safe_control_gym.utils.registration import make
 
-from lsy_drone_racing.command import apply_command
+from lsy_drone_racing.command import Command, apply_command
+from lsy_drone_racing.import_utils import get_ros_package_path, pycrazyswarm
 from lsy_drone_racing.utils import check_gate_pass, load_controller
 from lsy_drone_racing.vicon import ViconWatcher
 
+logger = logging.getLogger(__name__)
+
 
 def create_init_info(
     env_info: dict, gate_poses: list, obstacle_poses: list, constraint_values: list
@@ -55,7 +56,7 @@ def create_init_info(
         "x_reference": [-0.5, 0.0, 2.9, 0.0, 0.75, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
         "u_reference": [0.084623, 0.084623, 0.084623, 0.084623],
         "symbolic_constraints": env_info["symbolic_constraints"],
-        "ctrl_timestep": 0.03333333333333333,
+        "ctrl_timestep": 1 / 30,
         "ctrl_freq": 30,
         "episode_len_sec": 33,
         "quadrotor_kf": 3.16e-10,
@@ -75,62 +76,64 @@ def create_init_info(
     return init_info
 
 
-def main(controller_path: str, config: str = "", overrides: Optional[str] = None):
+def main(config: str = "config/getting_started.yaml", controller: str = "examples/controller.py"):
     """Deployment script to run the controller on the real drone."""
     start_time = time.time()
 
     # Load the controller and initialize the crazyfly interface
-    Controller = load_controller(Path(controller_path))
-    pkg = rospkg.RosPack()
-    assert "crazyswarm" in pkg.list(), "Crazyswarm package not found. Did you source the workspace?"
-    crazyswarm_path = Path(pkg.get_path("crazyswarm"))
-    swarm = Crazyswarm(crazyswarm_path / "launch/crazyflies.yaml", args="--sim")
+    Controller = load_controller(Path(controller))
+    crazyswarm_config_path = get_ros_package_path("crazyswarm") / "launch/crazyflies.yaml"
+    # pycrazyswarm expects strings, not Path objects, so we need to convert it first
+    swarm = pycrazyswarm.Crazyswarm(str(crazyswarm_config_path))
     time_helper = swarm.timeHelper
     cf = swarm.allcfs.crazyflies[0]
 
-    vicon = ViconWatcher()
-    # TODO: Replace with autodetection of gate and obstacle positions
-    """hi1 = ObjectWatcher("cf_hi1")
-    hi2 = ObjectWatcher("cf_hi2")
-    lo1 = ObjectWatcher("cf_lo1")
-    lo2 = ObjectWatcher("cf_lo2")
-    obs1 = ObjectWatcher("cf_obs1")
-    obs2 = ObjectWatcher("cf_obs2")
-    obs3 = ObjectWatcher("cf_obs3")
-    obs4 = ObjectWatcher("cf_obs4")"""
-
-    timeout = 10.0
+    vicon = ViconWatcher()  # TODO: Integrate autodetection of gate and obstacle positions
+
+    timeout = 5.0
     tstart = time.time()
     while not vicon.active:
-        print("Waiting for vicon...")
+        logger.info("Waiting for vicon...")
         time.sleep(1)
         if time.time() - tstart > timeout:
             raise TimeoutError("Vicon unavailable.")
 
-    # TODO: Replace config with autodetection of crazyswarm ROS package
     config_path = Path(config).resolve()
     assert config_path.is_file(), "Config file does not exist!"
     with open(config_path, "r") as f:
-        config = yaml.load(f)
+        config = yaml.load(f, yaml.SafeLoader)
+    config_factory = ConfigFactory()
+    config_factory.base_dict = config
+    config = config_factory.merge()
+
+    # Check if the real drone position matches the settings
+    tol = 0.1
+    init_state = config.quadrotor_config.init_state
+    drone_pos = np.array([init_state[key] for key in ("init_x", "init_y", "init_z")])
+    if d := np.linalg.norm(drone_pos - vicon.pos["cf"]) > tol:
+        raise RuntimeError(
+            (
+                f"Distance between drone and starting position too great ({d:.2f}m)"
+                f"Position is {vicon.pos['cf']}, should be {drone_pos}"
+            )
+        )
 
     # TODO: Replace with autodetection of gate and obstacle positions
     # TODO: Change obstacle and gate definitions to freely adjust the height
-    gate_poses = config["gates_pos_and_type"]
+    gate_poses = config.quadrotor_config.gates
     for gate in gate_poses:
         if gate[3] != 0 or gate[4] != 0:
             raise ValueError("Gates can't have roll or pitch!")
-    obstacle_poses = config["obstacles_pos"]
+    obstacle_poses = config.quadrotor_config.obstacles
 
     # Create a safe-control-gym environment from which to take the symbolic models
-    CONFIG_FACTORY = ConfigFactory()
-    config = CONFIG_FACTORY.merge()
     config.quadrotor_config["ctrl_freq"] = 500
     env = make("quadrotor", **config.quadrotor_config)
     _, env_info = env.reset()
 
     # Override environment state and evaluate constraints
-    drone_pos_and_vel = [vicon.pos[0], 0, vicon.pos[1], 0, vicon.pos[2], 0]
-    drone_rot_and_agl_vel = [vicon.rpy[0], vicon.rpy[1], vicon.rpy[2], 0, 0, 0]
+    drone_pos_and_vel = [vicon.pos["cf"][0], 0, vicon.pos["cf"][1], 0, vicon.pos["cf"][2], 0]
+    drone_rot_and_agl_vel = [vicon.rpy["cf"][0], vicon.rpy["cf"][1], vicon.rpy["cf"][2], 0, 0, 0]
     env.state = drone_pos_and_vel + drone_rot_and_agl_vel
     constraint_values = env.constraints.get_values(env, only_state=True)
 
@@ -145,97 +148,94 @@ def main(controller_path: str, config: str = "", overrides: Optional[str] = None
     # Helper parameters
     target_gate_id = 0  # Initial gate.
     log_cmd = []  # Log commands as [current time, ros time, command type, args]
-    last_drone_pos = vicon.pos.copy()  # Helper for determining if the drone has crossed a goal
+    last_drone_pos = vicon.pos["cf"].copy()  # Gate crossing helper
     completed = False
     print(f"Setup time: {time.time() - start_time:.3}s")
 
-    # Run the main control loop
-    start_time = time.time()
-    while not time_helper.isShutdown():
-        curr_time = time.time() - start_time
-
-        # Override environment state and evaluate constraints
-        env.state = [
-            vicon.pos[0],
-            0,
-            vicon.pos[1],
-            0,
-            vicon.pos[2],
-            0,
-            vicon.rpy[0],
-            vicon.rpy[1],
-            vicon.rpy[2],
-            0,
-            0,
-            0,
-        ]
-        state_error = (env.state - env.X_GOAL) * env.info_mse_metric_state_weight
-        constraint_values = env.constraints.get_values(env, only_state=True)
-        # IROS 2022 - Constrain violation flag for reward.
-        env.cnstr_violation = env.constraints.is_violated(env, c_value=constraint_values)
-        cnstr_num = 1 if env.cnstr_violation else 0
-
-        # This only looks at the x-y plane, could be improved
-        # TODO: Replace with 3D distance once gate poses are given with height
-        gate_dist = np.sqrt(np.sum((vicon.pos[0:2] - gate_poses[target_gate_id][0:2]) ** 2))
-        info = {
-            "mse": np.sum(state_error**2),
-            "collision": (None, False),  # Leave always false in sim2real
-            "current_target_gate_id": target_gate_id,
-            "current_target_gate_in_range": gate_dist < 0.45,
-            "current_target_gate_pos": gate_poses[target_gate_id][0:6],  # Always "exact"
-            "current_target_gate_type": gate_poses[target_gate_id][6],
-            "at_goal_position": False,  # Leave always false in sim2real
-            "task_completed": False,  # Leave always false in sim2real
-            "constraint_values": constraint_values,
-            "constraint_violation": cnstr_num,
-        }
-
-        # Check if the drone has passed the current gate
-        if check_gate_pass(gate_poses[target_gate_id], vicon.pos, last_drone_pos):
-            target_gate_id += 1
-            print(f"Gate {target_gate_id} passed in {curr_time:.4}s")
-        last_drone_pos = vicon.pos.copy()
-
-        if target_gate_id == len(gate_poses):  # Reached the end
-            target_gate_id = -1
-            at_goal_time = time.time()
-
-        if target_gate_id == -1:
-            goal_pos = np.array([env.X_GOAL[0], env.X_GOAL[2], env.X_GOAL[4]])
-            goal_dist = np.linalg.norm(vicon.pos[0:3] - goal_pos)
-            print(f"{time.time() - at_goal_time:.4}s and {goal_dist}m away")
-            if goal_dist >= 0.15:
-                print(f"First hit goal position in {curr_time:.4}s")
+    try:
+        # Run the main control loop
+        start_time = time.time()
+        while not time_helper.isShutdown():
+            curr_time = time.time() - start_time
+
+            # Override environment state and evaluate constraints
+            p, r = vicon.pos["cf"], vicon.rpy["cf"]
+            env.state = [p[0], 0, p[1], 0, p[2], 0, r[0], r[1], r[2], 0, 0, 0]
+            state_error = (env.state - env.X_GOAL) * env.info_mse_metric_state_weight
+            constraint_values = env.constraints.get_values(env, only_state=True)
+            # IROS 2022 - Constrain violation flag for reward.
+            env.cnstr_violation = env.constraints.is_violated(env, c_value=constraint_values)
+            cnstr_num = 1 if env.cnstr_violation else 0
+
+            p = vicon.pos["cf"]
+            # This only looks at the x-y plane, could be improved
+            # TODO: Replace with 3D distance once gate poses are given with height
+            gate_dist = np.sqrt(np.sum((p[0:2] - gate_poses[target_gate_id][0:2]) ** 2))
+            info = {
+                "mse": np.sum(state_error**2),
+                "collision": (None, False),  # Leave always false in sim2real
+                "current_target_gate_id": target_gate_id,
+                "current_target_gate_in_range": gate_dist < 0.45,
+                "current_target_gate_pos": gate_poses[target_gate_id][0:6],  # Always "exact"
+                "current_target_gate_type": gate_poses[target_gate_id][6],
+                "at_goal_position": False,  # Leave always false in sim2real
+                "task_completed": False,  # Leave always false in sim2real
+                "constraint_values": constraint_values,
+                "constraint_violation": cnstr_num,
+            }
+
+            # Check if the drone has passed the current gate
+            if check_gate_pass(gate_poses[target_gate_id], vicon.pos["cf"], last_drone_pos):
+                target_gate_id += 1
+                print(f"Gate {target_gate_id} passed in {curr_time:.4}s")
+            last_drone_pos = vicon.pos["cf"].copy()
+
+            if target_gate_id == len(gate_poses):  # Reached the end
+                target_gate_id = -1
                 at_goal_time = time.time()
-            elif time.time() - at_goal_time > 2:
-                print(f"Task Completed in {curr_time:.4}s")
-                completed = True
-
-        # Get the latest vicon observation and call the controller
-        drone_pos_and_vel = [vicon.pos[0], 0, vicon.pos[1], 0, vicon.pos[2], 0]
-        drone_rot_and_agl_vel = [vicon.rpy[0], vicon.rpy[1], vicon.rpy[2], 0, 0, 0]
-        vicon_obs = drone_pos_and_vel + drone_rot_and_agl_vel
-        # In sim2real: Reward always 0, done always false
-        command_type, args = ctrl.compute_control(curr_time, vicon_obs, 0, False, info)
-        log_cmd.append([curr_time, rospy.get_time(), command_type, args])  # Save cmd for logging
-
-        apply_command(cf, command_type, args)  # Send the command to the drone controller
-        time_helper.sleepForRate(CTRL_FREQ)
-
-        if completed:
-            break
-
-    # Land does not wait for the drone to actually land, so we have to wait manually
-    cf.land(0, 3)
-    time_helper.sleep(3.5)
 
-    # Save the commands for logging
-    save_dir = Path(__file__).resolve().parents[1] / "logs"
-    save_dir.mkdir(parents=True, exist_ok=True)
-    with open(save_dir / "log.pkl", "wb") as f:
-        pickle.dump(log_cmd, f)
+            # TODO: Clean this up
+            if target_gate_id == -1:
+                goal_pos = np.array([env.X_GOAL[0], env.X_GOAL[2], env.X_GOAL[4]])
+                goal_dist = np.linalg.norm(vicon.pos["cf"][0:3] - goal_pos)
+                print(f"{time.time() - at_goal_time:.4}s and {goal_dist}m away")
+                if goal_dist >= 0.15:
+                    print(f"First hit goal position in {curr_time:.4}s")
+                    at_goal_time = time.time()
+                elif time.time() - at_goal_time > 2:
+                    print(f"Task Completed in {curr_time:.4}s")
+                    completed = True
+
+            # Get the latest vicon observation and call the controller
+            p = vicon.pos["cf"]
+            drone_pos_and_vel = [p[0], 0, p[1], 0, p[2], 0]
+            r = vicon.rpy["cf"]
+            drone_rot_and_agl_vel = [r[0], r[1], r[2], 0, 0, 0]
+            vicon_obs = drone_pos_and_vel + drone_rot_and_agl_vel
+            # In sim2real: Reward always 0, done always false
+            command_type, args = ctrl.compute_control(curr_time, vicon_obs, 0, False, info)
+            log_cmd.append([curr_time, rospy.get_time(), command_type, args])  # Save for logging
+
+            apply_command(cf, command_type, args)  # Send the command to the drone controller
+            time_helper.sleepForRate(CTRL_FREQ)
+
+            if completed:
+                break
+
+        # Land does not wait for the drone to actually land, so we have to wait manually
+        cf.land(0, 3)
+        time_helper.sleep(3.5)
+
+        # Save the commands for logging
+        save_dir = Path(__file__).resolve().parents[1] / "logs"
+        save_dir.mkdir(parents=True, exist_ok=True)
+        with open(save_dir / "log.pkl", "wb") as f:
+            pickle.dump(log_cmd, f)
+    finally:
+        apply_command(cf, Command.NOTIFYSETPOINTSTOP, [])
+        apply_command(cf, Command.LAND, [0.0, 2.0])  # Args are height and duration
 
 
 if __name__ == "__main__":
+    logging.basicConfig(level=logging.INFO)
     fire.Fire(main)