diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a771797..d7d60a4 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -56,4 +56,15 @@ jobs: cat launch.log grep -q 'crazyflies at' launch.log || { echo "crazyflie_server did not start"; exit 1; } grep -q 'ground robots at' launch.log || { echo "ground_robot_sim did not start"; exit 1; } + if grep -q 'tether_sim' launch.log; then echo "tether_sim started without a tether: section"; exit 1; fi if grep -E 'Traceback|process has died' launch.log; then exit 1; fi + + - name: Tether smoke test + working-directory: ws + run: | + source install/setup.bash + printf 'ground_robots: [{name: r1}]\ncrazyflies: [{pose: {}}]\ntether: {winch: {x: -1, y: 0}, object: {x: 1, y: 0}, goal: {x: 2, y: 0}}\nsim: {rviz: false}\n' > /tmp/tether.yaml + timeout 10 ros2 launch hero_sim sim.launch.py config:=/tmp/tether.yaml > tether.log 2>&1 || true + cat tether.log + grep -q 'tether ready' tether.log || { echo "tether_sim did not start"; exit 1; } + if grep -E 'Traceback|process has died' tether.log; then exit 1; fi diff --git a/README.md b/README.md index e791459..591ad66 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,8 @@ - # hero_sim [![CI](https://github.com/KhAlamdar11/hero_sim/actions/workflows/ci.yml/badge.svg)](https://github.com/KhAlamdar11/hero_sim/actions/workflows/ci.yml) -Lightweight HERO simulation without Gazebo. Crazyflies and ground robots are simulated kinematically (velocities are integrated straight into poses), with the same ROS 2 topics and services the control stack uses. +A lightweight simulator for the HERO robots: Crazyflie drones and ground robots (herover, scout), shown in RViz. There is no physics engine, so robots move exactly as commanded. That makes it quick to start and easy to test your code. You command the robots through the same ROS 2 topics and services as the real ones.

@@ -11,104 +10,200 @@ Lightweight HERO simulation without Gazebo. Crazyflies and ground robots are sim ## Setup -Runs inside the `icuas26_finals` docker container. +You need the `icuas26_finals` docker container. Run every command in this README inside it. -1. Mount this repo in the container. `~/dockers/icuas26_finals/first_run.sh` already has the line below; recreate the container if it was created before that line was added: +1. Get the code into your workspace: ```bash - --volume /home/khawaja/HERO/hero_sim:/root/ros2_ws/src/hero_sim:rw \ + cd ~/ros2_ws/src + git clone https://github.com/KhAlamdar11/hero_sim.git ``` -2. Build (inside the container): +2. Build it, together with the Crazyflie messages it needs: ```bash cd ~/ros2_ws - colcon build --symlink-install --packages-select hero_sim - source install/setup.bash + colcon build --symlink-install --packages-up-to hero_sim ``` +3. Open a new terminal, which loads the workspace automatically, or run `source ~/ros2_ws/install/setup.bash` in this one. -Requires `crazyflie_interfaces` (crazyswarm2) in the same workspace. - -Optional: if `hero_mrs_msgs` (from hero_mrs_control_stack) is also in the workspace, the dock ride-along node (`/sim/attach_agent`) starts automatically. Without it, that node is skipped and everything else runs normally. Called with an empty `parent_id`, the same service locks a UAV in place instead (used for a UAV hanging from a branch perch, see [Grippers](#grippers)). +After you pull new changes, run step 2 again. -## Usage +## Start and stop ```bash cd ~/ros2_ws/src/hero_sim/startup +bash start.sh tether_demo.yaml # start a scenario +tmux kill-session -t hero_sim # stop +``` -bash start.sh # default config -bash start.sh test_many.yaml # a config from config/ -bash start.sh /path/to/my.yaml # any config file +`start.sh` opens a tmux session with the simulator and RViz. Without a scenario name, it starts the default one, set by `DEFAULT_CONFIG_NAME` at the top of `startup/_setup.sh`. -tmux kill-session -t hero_sim # stop -``` +To run it without tmux, use `ros2 launch hero_sim sim.launch.py config:=tether_demo.yaml`. The map isn't drawn in RViz then, but robots still bump into it. + +## Scenarios -This opens a tmux session running the sim (CF server, ground robots, attach node, RViz) and the octomap server. +A scenario is one YAML file in `config/`. It says which robots there are, where they start, and what's in the world. -To change the default config, edit `DEFAULT_CONFIG_NAME` at the top of `startup/_setup.sh`. +| Where | What | +|---|---| +| `config/tether/` | the tether task | +| `config/mrs/` | multi-robot scenarios with Crazyflies | +| `config/` | the example and everything else | -## Config +Start a scenario by its filename. It's found in whichever folder it's in: `bash start.sh test_many.yaml`. You can also name the folder (`mrs/test_many.yaml`) or give the path to a file anywhere. -One YAML file per scenario in `config/`. To make a new one, copy `config/example.yaml`, which explains every option. +To make your own, copy `config/example.yaml` (it explains every option) to, say, `config/my_test.yaml`. Edit it and run `bash start.sh my_test.yaml`. Filenames must be unique across the folders. + +A small scenario: ```yaml ground_robots: - - name: scout_1 - pose: {x: 0.0, y: 0.0, yaw: 0.0} - drive: omni # omni or diff - model: scout # 3D model in RViz: scout or herover; leave out for TF axes only - name: herover_1 - pose: {x: 0.0, y: 0.6, yaw: 0.0} - drive: omni - model: herover + pose: {x: 0.0, y: 0.0, yaw: 0.0} # m, m, rad + drive: omni # omni (can move sideways) or diff (can't) + model: herover # herover or scout crazyflies: # named cf_1, cf_2, ... in this order - - pose: {x: 1.5, y: 0.5, z: 0.03} # cf_1 - - pose: {x: 1.5, y: -0.5, z: 0.03} # cf_2 + - pose: {x: 1.0, y: 0.0, z: 0.03} + +obstacles: # pillars instead of a map, see Maps below + - {x: 2.0, y: 1.0, radius: 0.3} +``` + +## Maps + +What's in the world comes from one of two places: -sim: # optional, all keys have defaults - rviz: true - models: true # robot 3D models in RViz; false = TF axes only - map: icuas26_1 # octomap from maps/, or null for none +| | Set it with | In RViz | +|---|---|---| +| A map | `sim: {map: icuas26_1}`: a map from `maps/`, or the path to any octomap `.bt` file. `icuas26_1` is the default | occupied cells | +| Pillars | an `obstacles:` list, on an empty floor | occupied cells, plus a cylinder for each pillar | + +A scenario uses one or the other: with `obstacles:` there's no map, and setting both is an error. For an empty floor with nothing on it, set `sim: {map: null}` and leave out `obstacles:`. + +```yaml +obstacles: # m + - {x: 2.0, y: 1.0, radius: 0.3} + - {x: 3.0, y: -1.0, radius: 0.2, height: 0.5} # height: how tall it is (2 m if left out) ``` -## Robot models in RViz +The pillars are turned into an octomap, written to `/tmp/hero_sim/_obstacles.bt`, and served just like a `.bt` map, with the same topics (`/octomap_binary`, `/projected_map`, ...). So RViz, and any code that reads the map, can't tell them apart. To keep a pillar map, copy that file and use it as `sim: {map: path/to/file.bt}` in another scenario. + +The map is drawn when you start with `start.sh`. Both kinds block the robots, see [Obstacles](#obstacles). + +## Moving the robots -Crazyflies, and ground robots with a `model:`, are drawn as 3D models that follow their TF frames. To hide them, untick the **Robot models** display in RViz, or set `sim: {models: false}` to not load them at all. +Every robot's pose is on `//pose` (`geometry_msgs/PoseStamped`, in the `world` frame) and on TF. -The meshes in `models//` are simplified for RViz (herover 309k → 20k triangles, scout 349k → 12k). To add or re-make one, put the raw mesh in `models/source/` (not committed) and run: +### Ground robots + +| Name | Type | | +|---|---|---| +| `//cmd_vel` | topic, `geometry_msgs/Twist` | velocity in the robot's own frame. Kept until the next message, so one is enough | +| `//go_to` | service, `crazyflie_interfaces/srv/GoTo` | drive to x, y and yaw in about `duration` seconds | ```bash -blender -b --factory-startup -P models/bake_mesh.py -- \ - "models/source/herover/hero mini rover.obj" models/herover/herover.glb 6000 -90 Y +ros2 topic pub --once /herover_1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5}}" +ros2 topic pub --once /herover_1/cmd_vel geometry_msgs/msg/Twist "{}" # stop +ros2 service call /herover_1/go_to crazyflie_interfaces/srv/GoTo \ + "{goal: {x: 1.0, y: -1.0}, yaw: 1.57, duration: {sec: 3}}" ``` -Robots made of several `.dae` parts (like the scout, from agilex_sitl's meshes) are first combined into one OBJ with `models/assemble_dae.py`; its header has the command. Then register the mesh in `MODEL_MESHES` in `hero_sim/config.py`. +- Speed is limited to 2 m/s and turning to 2 rad/s. A `go_to` that asks to go faster just takes longer. +- Any `cmd_vel` message cancels a `go_to`. +- `go_to` exists only in the simulator. On the real ground robots, use `cmd_vel`. +- A ground robot's pose height is the top of its deck, where the motion-capture markers sit on the real robot, not 0. -## Grippers +### Crazyflies -A Crazyflie with `gripper: true` carries a two-claw gripper on top, drawn in RViz and open at the start: +The same services as the real Crazyflies (crazyswarm2): -```yaml -crazyflies: - - pose: {x: 1.5, y: 0.5, z: 0.03} - gripper: true +| Name | Type | | +|---|---|---| +| `/cf_1/takeoff` | `crazyflie_interfaces/srv/Takeoff` | rise to `height` | +| `/cf_1/go_to` | `crazyflie_interfaces/srv/GoTo` | fly to x, y, z and yaw in `duration` | +| `/cf_1/land` | `crazyflie_interfaces/srv/Land` | come down to `height` | +| `/all/takeoff`, `/all/go_to`, `/all/land` | | the same, for every Crazyflie at once | + +```bash +ros2 service call /cf_1/takeoff crazyflie_interfaces/srv/Takeoff "{height: 1.0, duration: {sec: 2}}" +ros2 service call /cf_1/go_to crazyflie_interfaces/srv/GoTo "{goal: {x: 1.0, y: 1.0, z: 1.0}, duration: {sec: 3}}" +ros2 service call /cf_1/land crazyflie_interfaces/srv/Land "{height: 0.0, duration: {sec: 2}}" ``` -Locking the UAV in place (`/sim/attach_agent` with an empty `parent_id`, as when it hangs from a branch) closes the gripper. Detaching leaves the gripper on the branch, and the UAV flies off without it. To fit a new one: +The low-level setpoint topics (`cmd_position`, `cmd_hover`, ...) work too. + +## Obstacles + +A robot stops when it would hit something, and moves again once you command it away. These block it: + +- the map's trunks and posts; +- pillars, from the scenario's `obstacles:` list; +- other ground robots. + +Drones are blocked at any height, even above a short pillar: they have to fly around a trunk or a pillar, not over it. Drones don't collide with ground robots or with each other. + +Each robot is a circle, and its size is its `radius:`, which you can set per robot (defaults: herover 0.17 m, scout 0.42 m, Crazyflie 0.1 m). To turn all collisions off, set `sim: {collision: false}`. + +Your code can read the pillars from `/hero_sim/obstacles` (`visualization_msgs/MarkerArray`). There's one cylinder per pillar: `pose.position` is its center, `scale.x` its diameter and `scale.z` its height. ```bash -ros2 service call /sim/cf_1/add_gripper std_srvs/srv/Trigger +ros2 topic echo --once /hero_sim/obstacles ``` -The attach node runs the grippers, so they also need `hero_mrs_msgs`. Like the robot models, they are hidden with `sim: {models: false}`. +## Tether task -## Driving the robots +Get an object into a goal circle on the floor. A tether runs from a fixed winch, through a pulley on a ground robot (UGV), through a ring on a Crazyflie (UAV), and down to the object. + +

+ +

```bash -# ground robot -ros2 topic pub /scout_1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5}}" +bash start.sh tether_demo.yaml +``` -# crazyflie -ros2 service call /cf_1/takeoff crazyflie_interfaces/srv/Takeoff "{height: 1.0, duration: {sec: 2}}" -ros2 service call /all/land crazyflie_interfaces/srv/Land "{height: 0.0, duration: {sec: 2}}" +How the tether behaves: + +- The winch pays out a tether length L. The part hanging below the UAV is l = L − |winch − UGV| − |UGV − UAV|. +- While the tether is slack, the object stays where it is. +- Once it's taut, the object slides along the floor toward the point under the UAV. +- The object is never lifted. Reeling in further only makes the winch slip. +- Pulled into an obstacle, the object gets stuck until you pull it away from it. +- The robots don't feel the tether, and the tether line passes through obstacles. Only the object gets stuck. + +Drive the UGV and fly the UAV as in [Moving the robots](#moving-the-robots). The winch and the object: + +| Topic | Type | | +|---|---|---| +| `/winch/cmd_rate` | `std_msgs/Float64` | your input, m/s: positive pays out, negative reels in. Kept until the next message | +| `/winch/length` | `std_msgs/Float64` | L | +| `/tether/free_length` | `std_msgs/Float64` | l | +| `/tether/taut` | `std_msgs/Bool` | the tether is taut | +| `/tether/stuck` | `std_msgs/Bool` | the object is stuck against an obstacle | +| `/tether/object_pose` | `geometry_msgs/PoseStamped` | the object's center | +| `/tether/goal` | `geometry_msgs/PoseStamped` | the goal's center | +| `/tether/goal_reached` | `std_msgs/Bool` | the object's center is inside the goal circle | + +```bash +ros2 topic pub --once /winch/cmd_rate std_msgs/msg/Float64 "{data: -0.3}" # reel in +ros2 topic pub --once /winch/cmd_rate std_msgs/msg/Float64 "{data: 0.0}" # hold ``` -Ground robots hold the last `cmd_vel` until a new one arrives. Poses are published on `//pose` and as TF from `world`. \ No newline at end of file +The `tether:` section of `config/tether/tether_demo.yaml` sets where the winch, the object and the goal are, and which robots carry the tether. `config/example.yaml` explains each key. + +## Troubleshooting + +| Problem | What to do | +|---|---| +| `config not found` | Check the filename: `ls ~/ros2_ws/src/hero_sim/config/*` | +| `sim.map and obstacles: are both set` | A scenario has a map or obstacles, not both: remove one | +| `... is in more than one folder` | Two configs share a filename: name the folder (`mrs/my.yaml`) or rename one. After configs were moved, an old build can also cause it: `rm -rf ~/ros2_ws/build/hero_sim ~/ros2_ws/install/hero_sim`, then build again | +| `Package 'hero_sim' not found` | Build ([Setup](#setup) step 2), then open a new terminal | +| `ros2 launch` doesn't find a config you just added | Build again. `start.sh` finds new files without a rebuild | +| A robot doesn't move | It may be pressed against an obstacle: command it away. Check that the simulator is running: `ros2 node list` | +| A `go_to` stops right away | Something keeps publishing `cmd_vel` to that robot, such as a `ros2 topic pub` without `--once` | + +## For hero_mrs_control_stack users + +- With `hero_mrs_msgs` in the workspace, `/sim/attach_agent` starts: robots carry docked Crazyflies, and an empty `parent_id` locks a Crazyflie in place (perched on a branch). +- `gripper: true` on a Crazyflie draws a two-claw gripper. To fit a new one: `ros2 service call /sim/cf_1/add_gripper std_srvs/srv/Trigger`. +- `sim: {models: false}` hides the robot models in RViz. The mesh tools are described in [models/README.md](models/README.md). diff --git a/config/example.yaml b/config/example.yaml index c8ffb84..24638e8 100644 --- a/config/example.yaml +++ b/config/example.yaml @@ -3,10 +3,19 @@ # Kinematic ground robots. Each one subscribes to //cmd_vel (body-frame # Twist, held until the next message) and publishes //pose plus TF -# world -> at z = 0. +# world -> at the pose's z. +# pose: {z: 0.227} height (m) of the published pose: where the mocap markers sit on +# the real robot. Default by model, the top of its deck: scout 0.227, +# herover 0.095, no model 0. The mesh stays on the floor either way # drive: omni holonomic, honors linear.x, linear.y and angular.z (default) # drive: diff non-holonomic, linear.y is ignored # model: scout | herover RViz mesh drawn on the robot's frame; leave out for TF axes only +# radius: 0.42 footprint (m) kept out of the map, pillars and other ground robots +# (sim.collision). Default by model: scout 0.42, herover 0.17, no +# model 0.3. 0 ignores them +# Each one also serves //go_to (crazyflie_interfaces/srv/GoTo, like the +# Crazyflies): drives to goal x, y and yaw in `duration`, slower if the speed +# limits below need it. Any cmd_vel message cancels it. ground_robots: - name: scout_1 pose: {x: 0.0, y: 0.0, yaw: 0.0} # m, m, rad @@ -22,6 +31,7 @@ ground_robots: # gripper: true starts with an open two-claw gripper on top, drawn in RViz # (default false). Needs sim_attach (see attach below), which # shuts it on a branch lock and serves /sim//add_gripper. +# radius: 0.1 footprint (m) kept out of the map and pillars (default 0.1) crazyflies: - pose: {x: 1.5, y: 0.5, z: 0.03} # cf_1 gripper: true @@ -30,12 +40,40 @@ crazyflies: # Optional. Leave out any key to keep its default. sim: ground_rate: 50.0 # Hz, ground robot integration + /pose rate - ground_max_speed: 2.0 # m/s, clamp on |(linear.x, linear.y)| - ground_max_yaw_rate: 2.0 # rad/s, clamp on |angular.z| + ground_max_speed: 2.0 # m/s, clamp on |(linear.x, linear.y)| and go_to speed + ground_max_yaw_rate: 2.0 # rad/s, clamp on |angular.z| and go_to turns cf_rate: 10.0 # Hz, Crazyflie update + /pose rate cf_battery_drain: 0.025 # V/s while flying (starts 4.2 V, floors at 3.0 V) attach: auto # sim_attach (/sim/attach_agent dock ride-along): auto runs it # only if hero_mrs_msgs is installed; true | false to force rviz: true # RViz with config/sim.rviz models: true # robot meshes in RViz (crazyflies, and ground robots with a model) - map: icuas26_1 # octomap: maps/.bt, a path to a .bt, or null for none + # map: icuas26_1 # octomap: maps/.bt, a path to a .bt, or null for none. + # Default icuas26_1; not together with obstacles: (below) + collision: true # robots and the tether object stop where their footprint + # would hit a map obstacle or a pillar (obstacles: below); + # ground robots also stop at each other + collision_z: [0.2, 2.0] # m, map heights that count as obstacles, flattened to 2D: + # trunks and posts, not the floor, ceiling or tree canopies + +# Optional pillars (m): circles that block the robots (drones at any height) +# and the tether object while sim.collision is on. They are the scenario's +# map: no sim.map with them (setting both is an error), and RViz shows them +# as an octomap like a .bt map's. Also drawn as cylinders and published +# latched on /hero_sim/obstacles. +# height: 2.0 how tall it is in RViz and the octomap (default 2.0); collisions ignore it +obstacles: + - {x: 1.5, y: 1.5, radius: 0.35, height: 2.0} + +# Optional: a winch drags an object across the floor with a tether through a +# pulley on a ground robot and a ring on a Crazyflie (README "Tether task", +# config/tether/tether_demo.yaml). Leave it out and nothing extra runs. +# tether: +# winch: {x: -3.0, y: 0.0, z: 0.3} # m, x and y required; z: where the tether leaves the drum +# ugv: scout_1 # pulley on its deck (default: the first ground robot) +# uav: cf_1 # ring (default cf_1) +# object: {x: 0.5, y: -0.5} # required, start on the floor +# object_size: 0.3 # m, cube edge; half of it is its footprint against obstacles +# goal: {x: 2.5, y: 1.5, radius: 0.4} # x and y required +# slack: 0.5 # m of tether beyond taut at the start +# max_reel_speed: 0.5 # m/s, clamp on /winch/cmd_rate diff --git a/config/backup.yaml b/config/mrs/backup.yaml similarity index 100% rename from config/backup.yaml rename to config/mrs/backup.yaml diff --git a/config/collision_avoid.yaml b/config/mrs/collision_avoid.yaml similarity index 100% rename from config/collision_avoid.yaml rename to config/mrs/collision_avoid.yaml diff --git a/config/dock_hardware.yaml b/config/mrs/dock_hardware.yaml similarity index 100% rename from config/dock_hardware.yaml rename to config/mrs/dock_hardware.yaml diff --git a/config/dock_hardware2.yaml b/config/mrs/dock_hardware2.yaml similarity index 100% rename from config/dock_hardware2.yaml rename to config/mrs/dock_hardware2.yaml diff --git a/config/dock_redeploy12.yaml b/config/mrs/dock_redeploy12.yaml similarity index 100% rename from config/dock_redeploy12.yaml rename to config/mrs/dock_redeploy12.yaml diff --git a/config/dock_redeploy3.yaml b/config/mrs/dock_redeploy3.yaml similarity index 100% rename from config/dock_redeploy3.yaml rename to config/mrs/dock_redeploy3.yaml diff --git a/config/hardware_dock1.yaml b/config/mrs/hardware_dock1.yaml similarity index 100% rename from config/hardware_dock1.yaml rename to config/mrs/hardware_dock1.yaml diff --git a/config/hero_demo1.yaml b/config/mrs/hero_demo1.yaml similarity index 100% rename from config/hero_demo1.yaml rename to config/mrs/hero_demo1.yaml diff --git a/config/hero_sitl_config.yaml b/config/mrs/hero_sitl_config.yaml similarity index 100% rename from config/hero_sitl_config.yaml rename to config/mrs/hero_sitl_config.yaml diff --git a/config/herover.yaml b/config/mrs/herover.yaml similarity index 100% rename from config/herover.yaml rename to config/mrs/herover.yaml diff --git a/config/hw_dock_1.yaml b/config/mrs/hw_dock_1.yaml similarity index 100% rename from config/hw_dock_1.yaml rename to config/mrs/hw_dock_1.yaml diff --git a/config/hw_dock_2.yaml b/config/mrs/hw_dock_2.yaml similarity index 100% rename from config/hw_dock_2.yaml rename to config/mrs/hw_dock_2.yaml diff --git a/config/hw_dock_3.yaml b/config/mrs/hw_dock_3.yaml similarity index 100% rename from config/hw_dock_3.yaml rename to config/mrs/hw_dock_3.yaml diff --git a/config/hw_rs_2.yaml b/config/mrs/hw_rs_2.yaml similarity index 100% rename from config/hw_rs_2.yaml rename to config/mrs/hw_rs_2.yaml diff --git a/config/icuas.yaml b/config/mrs/icuas.yaml similarity index 100% rename from config/icuas.yaml rename to config/mrs/icuas.yaml diff --git a/config/integrated_test1.yaml b/config/mrs/integrated_test1.yaml similarity index 100% rename from config/integrated_test1.yaml rename to config/mrs/integrated_test1.yaml diff --git a/config/integrated_test2.yaml b/config/mrs/integrated_test2.yaml similarity index 100% rename from config/integrated_test2.yaml rename to config/mrs/integrated_test2.yaml diff --git a/config/integrated_test3.yaml b/config/mrs/integrated_test3.yaml similarity index 100% rename from config/integrated_test3.yaml rename to config/mrs/integrated_test3.yaml diff --git a/config/integrated_test4.yaml b/config/mrs/integrated_test4.yaml similarity index 100% rename from config/integrated_test4.yaml rename to config/mrs/integrated_test4.yaml diff --git a/config/peer_avoid.yaml b/config/mrs/peer_avoid.yaml similarity index 100% rename from config/peer_avoid.yaml rename to config/mrs/peer_avoid.yaml diff --git a/config/peer_avoid_orig.yaml b/config/mrs/peer_avoid_orig.yaml similarity index 100% rename from config/peer_avoid_orig.yaml rename to config/mrs/peer_avoid_orig.yaml diff --git a/config/perch_1_iso.yaml b/config/mrs/perch_1_iso.yaml similarity index 100% rename from config/perch_1_iso.yaml rename to config/mrs/perch_1_iso.yaml diff --git a/config/readme_fig.yaml b/config/mrs/readme_fig.yaml similarity index 100% rename from config/readme_fig.yaml rename to config/mrs/readme_fig.yaml diff --git a/config/renato.yaml b/config/mrs/renato.yaml similarity index 100% rename from config/renato.yaml rename to config/mrs/renato.yaml diff --git a/config/mrs/sim_env_a.yaml b/config/mrs/sim_env_a.yaml new file mode 100644 index 0000000..c0fcd7d --- /dev/null +++ b/config/mrs/sim_env_a.yaml @@ -0,0 +1,45 @@ +# sim_env_a: Env A of the simulation experiments — 4 x 4 m working area on a +# 5 x 5 m floor, scout_1 as the base in the (-x, -y) corner, 2 herovers with a +# dock each, 8 perch poles, +# 8 gripper CFs (hero_mrs_ros2 scenario sim_env_a; world from the stack's +# sim_experiments/worlds/make_sim_env_a.py). All keys, and the sim: block +# defaults, are documented in example.yaml. +ground_robots: + - name: scout_1 + pose: {x: -1.4, y: -1.4, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: -0.5, y: -1.4, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: -1.4, y: -0.5, z: 0.080, yaw: 0.0} + drive: omni + model: herover +# Every dock starts occupied (the stack's init_docked): cf_1..cf_4 on scout_1's +# dock_0..dock_3, cf_5 on herover_1's dock_4 and cf_6 on herover_2's dock_5 +# (z 0.23 = where a CF comes to rest on a herover dock; the stack attaches them +# so they ride along). cf_7 and cf_8 on the floor next to the base. Every CF +# carries a gripper; one left on a branch is replaced on a base dock. +# radius 0.07: a Crazyflie 2.1's real reach (motors 46 mm out + 22.5 mm props). +# The 0.10 default doesn't fit: the pads and branches facing 137.5 deg are +# 0.091 m from their voxelised pole, so collision would stop a CF 1 cm short. +crazyflies: + - {pose: {x: -1.513226, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_1, dock_0 + - {pose: {x: -1.513226, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_2, dock_1 + - {pose: {x: -1.286774, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_3, dock_2 + - {pose: {x: -1.286774, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_4, dock_3 + - {pose: {x: -0.5, y: -1.4, z: 0.23}, gripper: true, radius: 0.07} # cf_5, dock_4 (herover_1) + - {pose: {x: -1.4, y: -0.5, z: 0.23}, gripper: true, radius: 0.07} # cf_6, dock_5 (herover_2) + - {pose: {x: -1.85, y: -0.75, z: 0.03}, gripper: true, radius: 0.07} # cf_7 + - {pose: {x: -1.85, y: -0.5, z: 0.03}, gripper: true, radius: 0.07} # cf_8 +sim: + rviz: false # data runs; true to watch a pilot + models: false # meshes and grippers in RViz; true together with rviz + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/sim_env_a.bt + # Off since 2026-10-26 (P3e): a stop at a pole has no sliding along it, so a + # herover pushed against a pole froze there for up to 30 s. Robots now pass + # through poles; contacts are counted from the tracks instead. + # Platforms and branches aren't in the map, only the poles. + collision: false diff --git a/config/mrs/sim_env_a12.yaml b/config/mrs/sim_env_a12.yaml new file mode 100644 index 0000000..417b297 --- /dev/null +++ b/config/mrs/sim_env_a12.yaml @@ -0,0 +1,52 @@ +# sim_env_a12: sim_env_a.yaml with 12 CFs instead of 8 (hero_mrs_ros2 scenario +# sim_env_a12). cf_9..cf_12 start on the floor near the base, like cf_7 and cf_8, +# at least 0.4 m from any pole. Otherwise identical to sim_env_a.yaml. +# sim_env_a: Env A of the simulation experiments — 4 x 4 m working area on a +# 5 x 5 m floor, scout_1 as the base in the (-x, -y) corner, 2 herovers with a +# dock each, 8 perch poles, +# 8 gripper CFs (hero_mrs_ros2 scenario sim_env_a; world from the stack's +# sim_experiments/worlds/make_sim_env_a.py). All keys, and the sim: block +# defaults, are documented in example.yaml. +ground_robots: + - name: scout_1 + pose: {x: -1.4, y: -1.4, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: -0.5, y: -1.4, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: -1.4, y: -0.5, z: 0.080, yaw: 0.0} + drive: omni + model: herover +# Every dock starts occupied (the stack's init_docked): cf_1..cf_4 on scout_1's +# dock_0..dock_3, cf_5 on herover_1's dock_4 and cf_6 on herover_2's dock_5 +# (z 0.23 = where a CF comes to rest on a herover dock; the stack attaches them +# so they ride along). cf_7 and cf_8 on the floor next to the base. Every CF +# carries a gripper; one left on a branch is replaced on a base dock. +# radius 0.07: a Crazyflie 2.1's real reach (motors 46 mm out + 22.5 mm props). +# The 0.10 default doesn't fit: the pads and branches facing 137.5 deg are +# 0.091 m from their voxelised pole, so collision would stop a CF 1 cm short. +crazyflies: + - {pose: {x: -1.513226, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_1, dock_0 + - {pose: {x: -1.513226, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_2, dock_1 + - {pose: {x: -1.286774, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_3, dock_2 + - {pose: {x: -1.286774, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_4, dock_3 + - {pose: {x: -0.5, y: -1.4, z: 0.23}, gripper: true, radius: 0.07} # cf_5, dock_4 (herover_1) + - {pose: {x: -1.4, y: -0.5, z: 0.23}, gripper: true, radius: 0.07} # cf_6, dock_5 (herover_2) + - {pose: {x: -1.85, y: -0.75, z: 0.03}, gripper: true, radius: 0.07} # cf_7 + - {pose: {x: -1.85, y: -0.5, z: 0.03}, gripper: true, radius: 0.07} # cf_8 + - {pose: {x: -1.85, y: 0.0, z: 0.03}, gripper: true, radius: 0.07} # cf_9 + - {pose: {x: -1.85, y: 0.25, z: 0.03}, gripper: true, radius: 0.07} # cf_10 + - {pose: {x: 0.0, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_11 + - {pose: {x: -1.6, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_12 +sim: + rviz: false # data runs; true to watch a pilot + models: false # meshes and grippers in RViz; true together with rviz + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/sim_env_a.bt + # Off since 2026-10-26 (P3e): a stop at a pole has no sliding along it, so a + # herover pushed against a pole froze there for up to 30 s. Robots now pass + # through poles; contacts are counted from the tracks instead. + # Platforms and branches aren't in the map, only the poles. + collision: false diff --git a/config/mrs/sim_env_a12_floor.yaml b/config/mrs/sim_env_a12_floor.yaml new file mode 100644 index 0000000..4373da8 --- /dev/null +++ b/config/mrs/sim_env_a12_floor.yaml @@ -0,0 +1,50 @@ +# sim_env_a12_floor: sim_env_a_floor.yaml with 12 CFs (scenario sim_env_a12), for +# the conditions without docks or with the herovers' docks removed. Every CF +# starts on the floor; cf_9..cf_12 as in sim_env_a12.yaml. +# sim_env_a: Env A of the simulation experiments — 4 x 4 m working area on a +# 5 x 5 m floor, scout_1 as the base in the (-x, -y) corner, 2 herovers with a +# dock each, 8 perch poles, +# 8 gripper CFs (hero_mrs_ros2 scenario sim_env_a; world from the stack's +# sim_experiments/worlds/make_sim_env_a.py). All keys, and the sim: block +# defaults, are documented in example.yaml. +ground_robots: + - name: scout_1 + pose: {x: -1.4, y: -1.4, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: -0.5, y: -1.4, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: -1.4, y: -0.5, z: 0.080, yaw: 0.0} + drive: omni + model: herover +# Floor start, for the stack's E1 conditions without docks (overlays +# e1_no_dock, e1_bare): every CF starts on the floor next to the base, +# none on a dock. Otherwise identical to sim_env_a.yaml. +# radius 0.07: a Crazyflie 2.1's real reach (motors 46 mm out + 22.5 mm props). +# The 0.10 default doesn't fit: the pads and branches facing 137.5 deg are +# 0.091 m from their voxelised pole, so collision would stop a CF 1 cm short. +crazyflies: + - {pose: {x: -1.85, y: -1.0, z: 0.03}, gripper: true, radius: 0.07} # cf_1, floor + - {pose: {x: -1.85, y: -0.25, z: 0.03}, gripper: true, radius: 0.07} # cf_2, floor + - {pose: {x: -1.0, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_3, floor + - {pose: {x: -0.75, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_4, floor + - {pose: {x: -0.5, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_5, floor + - {pose: {x: -0.25, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_6, floor + - {pose: {x: -1.85, y: -0.75, z: 0.03}, gripper: true, radius: 0.07} # cf_7 + - {pose: {x: -1.85, y: -0.5, z: 0.03}, gripper: true, radius: 0.07} # cf_8 + - {pose: {x: -1.85, y: 0.0, z: 0.03}, gripper: true, radius: 0.07} # cf_9 + - {pose: {x: -1.85, y: 0.25, z: 0.03}, gripper: true, radius: 0.07} # cf_10 + - {pose: {x: 0.0, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_11 + - {pose: {x: -1.6, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_12 +sim: + rviz: false # data runs; true to watch a pilot + models: false # meshes and grippers in RViz; true together with rviz + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/sim_env_a.bt + # Off since 2026-10-26 (P3e): a stop at a pole has no sliding along it, so a + # herover pushed against a pole froze there for up to 30 s. Robots now pass + # through poles; contacts are counted from the tracks instead. + # Platforms and branches aren't in the map, only the poles. + collision: false diff --git a/config/mrs/sim_env_a12_view.yaml b/config/mrs/sim_env_a12_view.yaml new file mode 100644 index 0000000..2a99096 --- /dev/null +++ b/config/mrs/sim_env_a12_view.yaml @@ -0,0 +1,53 @@ +# COPY of sim_env_a12.yaml with RViz on, to watch a run. Keep the two in sync. +# sim_env_a12: sim_env_a.yaml with 12 CFs instead of 8 (hero_mrs_ros2 scenario +# sim_env_a12). cf_9..cf_12 start on the floor near the base, like cf_7 and cf_8, +# at least 0.4 m from any pole. Otherwise identical to sim_env_a.yaml. +# sim_env_a: Env A of the simulation experiments — 4 x 4 m working area on a +# 5 x 5 m floor, scout_1 as the base in the (-x, -y) corner, 2 herovers with a +# dock each, 8 perch poles, +# 8 gripper CFs (hero_mrs_ros2 scenario sim_env_a; world from the stack's +# sim_experiments/worlds/make_sim_env_a.py). All keys, and the sim: block +# defaults, are documented in example.yaml. +ground_robots: + - name: scout_1 + pose: {x: -1.4, y: -1.4, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: -0.5, y: -1.4, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: -1.4, y: -0.5, z: 0.080, yaw: 0.0} + drive: omni + model: herover +# Every dock starts occupied (the stack's init_docked): cf_1..cf_4 on scout_1's +# dock_0..dock_3, cf_5 on herover_1's dock_4 and cf_6 on herover_2's dock_5 +# (z 0.23 = where a CF comes to rest on a herover dock; the stack attaches them +# so they ride along). cf_7 and cf_8 on the floor next to the base. Every CF +# carries a gripper; one left on a branch is replaced on a base dock. +# radius 0.07: a Crazyflie 2.1's real reach (motors 46 mm out + 22.5 mm props). +# The 0.10 default doesn't fit: the pads and branches facing 137.5 deg are +# 0.091 m from their voxelised pole, so collision would stop a CF 1 cm short. +crazyflies: + - {pose: {x: -1.513226, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_1, dock_0 + - {pose: {x: -1.513226, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_2, dock_1 + - {pose: {x: -1.286774, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_3, dock_2 + - {pose: {x: -1.286774, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_4, dock_3 + - {pose: {x: -0.5, y: -1.4, z: 0.23}, gripper: true, radius: 0.07} # cf_5, dock_4 (herover_1) + - {pose: {x: -1.4, y: -0.5, z: 0.23}, gripper: true, radius: 0.07} # cf_6, dock_5 (herover_2) + - {pose: {x: -1.85, y: -0.75, z: 0.03}, gripper: true, radius: 0.07} # cf_7 + - {pose: {x: -1.85, y: -0.5, z: 0.03}, gripper: true, radius: 0.07} # cf_8 + - {pose: {x: -1.85, y: 0.0, z: 0.03}, gripper: true, radius: 0.07} # cf_9 + - {pose: {x: -1.85, y: 0.25, z: 0.03}, gripper: true, radius: 0.07} # cf_10 + - {pose: {x: 0.0, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_11 + - {pose: {x: -1.6, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_12 +sim: + rviz: true # watched run (sim_env_a12.yaml: the same with RViz off, for data runs) + models: true # meshes and grippers in RViz + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/sim_env_a.bt + # Off since 2026-10-26 (P3e): a stop at a pole has no sliding along it, so a + # herover pushed against a pole froze there for up to 30 s. Robots now pass + # through poles; contacts are counted from the tracks instead. + # Platforms and branches aren't in the map, only the poles. + collision: false diff --git a/config/mrs/sim_env_a_floor.yaml b/config/mrs/sim_env_a_floor.yaml new file mode 100644 index 0000000..d1bbd19 --- /dev/null +++ b/config/mrs/sim_env_a_floor.yaml @@ -0,0 +1,43 @@ +# sim_env_a: Env A of the simulation experiments — 4 x 4 m working area on a +# 5 x 5 m floor, scout_1 as the base in the (-x, -y) corner, 2 herovers with a +# dock each, 8 perch poles, +# 8 gripper CFs (hero_mrs_ros2 scenario sim_env_a; world from the stack's +# sim_experiments/worlds/make_sim_env_a.py). All keys, and the sim: block +# defaults, are documented in example.yaml. +ground_robots: + - name: scout_1 + pose: {x: -1.4, y: -1.4, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: -0.5, y: -1.4, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: -1.4, y: -0.5, z: 0.080, yaw: 0.0} + drive: omni + model: herover +# Floor start, for the stack's E1 conditions without docks (overlays +# e1_no_dock, e1_bare): every CF starts on the floor next to the base, +# none on a dock. Otherwise identical to sim_env_a.yaml. +# radius 0.07: a Crazyflie 2.1's real reach (motors 46 mm out + 22.5 mm props). +# The 0.10 default doesn't fit: the pads and branches facing 137.5 deg are +# 0.091 m from their voxelised pole, so collision would stop a CF 1 cm short. +crazyflies: + - {pose: {x: -1.85, y: -1.0, z: 0.03}, gripper: true, radius: 0.07} # cf_1, floor + - {pose: {x: -1.85, y: -0.25, z: 0.03}, gripper: true, radius: 0.07} # cf_2, floor + - {pose: {x: -1.0, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_3, floor + - {pose: {x: -0.75, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_4, floor + - {pose: {x: -0.5, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_5, floor + - {pose: {x: -0.25, y: -1.85, z: 0.03}, gripper: true, radius: 0.07} # cf_6, floor + - {pose: {x: -1.85, y: -0.75, z: 0.03}, gripper: true, radius: 0.07} # cf_7 + - {pose: {x: -1.85, y: -0.5, z: 0.03}, gripper: true, radius: 0.07} # cf_8 +sim: + rviz: false # data runs; true to watch a pilot + models: false # meshes and grippers in RViz; true together with rviz + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/sim_env_a.bt + # Off since 2026-10-26 (P3e): a stop at a pole has no sliding along it, so a + # herover pushed against a pole froze there for up to 30 s. Robots now pass + # through poles; contacts are counted from the tracks instead. + # Platforms and branches aren't in the map, only the poles. + collision: false diff --git a/config/mrs/sim_env_a_view.yaml b/config/mrs/sim_env_a_view.yaml new file mode 100644 index 0000000..ad2afd7 --- /dev/null +++ b/config/mrs/sim_env_a_view.yaml @@ -0,0 +1,46 @@ +# COPY of sim_env_a.yaml with RViz on, to watch a run. Keep the two in sync. +# sim_env_a: Env A of the simulation experiments — 4 x 4 m working area on a +# 5 x 5 m floor, scout_1 as the base in the (-x, -y) corner, 2 herovers with a +# dock each, 8 perch poles, +# 8 gripper CFs (hero_mrs_ros2 scenario sim_env_a; world from the stack's +# sim_experiments/worlds/make_sim_env_a.py). All keys, and the sim: block +# defaults, are documented in example.yaml. +ground_robots: + - name: scout_1 + pose: {x: -1.4, y: -1.4, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: -0.5, y: -1.4, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: -1.4, y: -0.5, z: 0.080, yaw: 0.0} + drive: omni + model: herover +# Every dock starts occupied (the stack's init_docked): cf_1..cf_4 on scout_1's +# dock_0..dock_3, cf_5 on herover_1's dock_4 and cf_6 on herover_2's dock_5 +# (z 0.23 = where a CF comes to rest on a herover dock; the stack attaches them +# so they ride along). cf_7 and cf_8 on the floor next to the base. Every CF +# carries a gripper; one left on a branch is replaced on a base dock. +# radius 0.07: a Crazyflie 2.1's real reach (motors 46 mm out + 22.5 mm props). +# The 0.10 default doesn't fit: the pads and branches facing 137.5 deg are +# 0.091 m from their voxelised pole, so collision would stop a CF 1 cm short. +crazyflies: + - {pose: {x: -1.513226, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_1, dock_0 + - {pose: {x: -1.513226, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_2, dock_1 + - {pose: {x: -1.286774, y: -1.490581, z: 0.23}, gripper: true, radius: 0.07} # cf_3, dock_2 + - {pose: {x: -1.286774, y: -1.309419, z: 0.23}, gripper: true, radius: 0.07} # cf_4, dock_3 + - {pose: {x: -0.5, y: -1.4, z: 0.23}, gripper: true, radius: 0.07} # cf_5, dock_4 (herover_1) + - {pose: {x: -1.4, y: -0.5, z: 0.23}, gripper: true, radius: 0.07} # cf_6, dock_5 (herover_2) + - {pose: {x: -1.85, y: -0.75, z: 0.03}, gripper: true, radius: 0.07} # cf_7 + - {pose: {x: -1.85, y: -0.5, z: 0.03}, gripper: true, radius: 0.07} # cf_8 +sim: + rviz: true # watched run (sim_env_a.yaml: the same with RViz off, for data runs) + models: true # meshes and grippers in RViz + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/sim_env_a.bt + # Off since 2026-10-26 (P3e): a stop at a pole has no sliding along it, so a + # herover pushed against a pole froze there for up to 30 s. Robots now pass + # through poles; contacts are counted from the tracks instead. + # Platforms and branches aren't in the map, only the poles. + collision: false diff --git a/config/test_9.yaml b/config/mrs/test_9.yaml similarity index 100% rename from config/test_9.yaml rename to config/mrs/test_9.yaml diff --git a/config/test_branch.yaml b/config/mrs/test_branch.yaml similarity index 100% rename from config/test_branch.yaml rename to config/mrs/test_branch.yaml diff --git a/config/test_deploy_2.yaml b/config/mrs/test_deploy_2.yaml similarity index 100% rename from config/test_deploy_2.yaml rename to config/mrs/test_deploy_2.yaml diff --git a/config/test_deploy_case2.yaml b/config/mrs/test_deploy_case2.yaml similarity index 100% rename from config/test_deploy_case2.yaml rename to config/mrs/test_deploy_case2.yaml diff --git a/config/test_many.yaml b/config/mrs/test_many.yaml similarity index 100% rename from config/test_many.yaml rename to config/mrs/test_many.yaml diff --git a/config/mrs/test_perching_full.yaml b/config/mrs/test_perching_full.yaml new file mode 100644 index 0000000..af20f89 --- /dev/null +++ b/config/mrs/test_perching_full.yaml @@ -0,0 +1,47 @@ +# test_perching_full: 20 CFs on the floor for the platform-swap stress test +# (hero_mrs_ros2 scenario test_perching_full: 16 platforms, no docks). +# Layout from test_deploy_2; all keys, and the sim: block defaults, are +# documented in example.yaml. + +ground_robots: + - name: scout_1 + pose: {x: 0.0, y: 0.0, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: 0.0, y: 0.6, yaw: 0.0} + drive: omni + model: herover + +# Two rows of 10, 0.2 m apart, south of the scout and clear of the poles +# at (+-1.5, +-1) and their platforms. +crazyflies: + - pose: {x: -0.9, y: -0.55, z: 0.03} # cf_1 + - pose: {x: -0.7, y: -0.55, z: 0.03} # cf_2 + - pose: {x: -0.5, y: -0.55, z: 0.03} # cf_3 + - pose: {x: -0.3, y: -0.55, z: 0.03} # cf_4 + - pose: {x: -0.1, y: -0.55, z: 0.03} # cf_5 + - pose: {x: 0.1, y: -0.55, z: 0.03} # cf_6 + - pose: {x: 0.3, y: -0.55, z: 0.03} # cf_7 + - pose: {x: 0.5, y: -0.55, z: 0.03} # cf_8 + - pose: {x: 0.7, y: -0.55, z: 0.03} # cf_9 + - pose: {x: 0.9, y: -0.55, z: 0.03} # cf_10 + - pose: {x: -0.9, y: -0.8, z: 0.03} # cf_11 + - pose: {x: -0.7, y: -0.8, z: 0.03} # cf_12 + - pose: {x: -0.5, y: -0.8, z: 0.03} # cf_13 + - pose: {x: -0.3, y: -0.8, z: 0.03} # cf_14 + - pose: {x: -0.1, y: -0.8, z: 0.03} # cf_15 + - pose: {x: 0.1, y: -0.8, z: 0.03} # cf_16 + - pose: {x: 0.3, y: -0.8, z: 0.03} # cf_17 + - pose: {x: 0.5, y: -0.8, z: 0.03} # cf_18 + - pose: {x: 0.7, y: -0.8, z: 0.03} # cf_19 + - pose: {x: 0.9, y: -0.8, z: 0.03} # cf_20 + +sim: + models: true # 20 CF meshes slow RViz down (test_many does the same) + # The perch world's poles (the platforms are not in the .bt). + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/perch2_world.bt + # Off: a perch approach flies straight with obstacle avoidance off, and + # hero_sim stops a drone at a trunk at any altitude, so a CF whose line to + # its platform crosses a pole would sit there. Turn on to see which do. + collision: false diff --git a/config/mrs/test_perching_full2.yaml b/config/mrs/test_perching_full2.yaml new file mode 100644 index 0000000..e72666b --- /dev/null +++ b/config/mrs/test_perching_full2.yaml @@ -0,0 +1,55 @@ +# test_perching_full2: 20 CFs on the floor, cf_1..cf_10 with a gripper, for +# branches and platforms together (hero_mrs_ros2 scenario test_perching_full2: +# branch and platform perches from the Blender generator, docks on scout_1 +# and both herovers). Layout from test_perching_full; all keys, and the sim: +# block defaults, are documented in example.yaml. + +ground_robots: + - name: scout_1 + pose: {x: 0.0, y: 0.0, z: 0.20, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: 0.0, y: 0.6, z: 0.080, yaw: 0.0} + drive: omni + model: herover + - name: herover_2 + pose: {x: 0.5, y: 0.6, z: 0.080, yaw: 0.0} + drive: omni + model: herover + +# Two rows of 10, 0.2 m apart, south of the scout and clear of the poles +# at (+-1.5, +-1). The front row carries the grippers (gripper.enabled in +# the stack's cf_1..cf_10.yaml); a gripper left on a branch is replaced on +# a base dock. +crazyflies: + - {pose: {x: -0.9, y: -0.55, z: 0.03}, gripper: true} # cf_1 + - {pose: {x: -0.7, y: -0.55, z: 0.03}, gripper: true} # cf_2 + - {pose: {x: -0.5, y: -0.55, z: 0.03}, gripper: true} # cf_3 + - {pose: {x: -0.3, y: -0.55, z: 0.03}, gripper: true} # cf_4 + - {pose: {x: -0.1, y: -0.55, z: 0.03}, gripper: true} # cf_5 + - {pose: {x: 0.1, y: -0.55, z: 0.03}, gripper: true} # cf_6 + - {pose: {x: 0.3, y: -0.55, z: 0.03}, gripper: true} # cf_7 + - {pose: {x: 0.5, y: -0.55, z: 0.03}, gripper: true} # cf_8 + - {pose: {x: 0.7, y: -0.55, z: 0.03}, gripper: true} # cf_9 + - {pose: {x: 0.9, y: -0.55, z: 0.03}, gripper: true} # cf_10 + - pose: {x: -0.9, y: -0.8, z: 0.03} # cf_11 + - pose: {x: -0.7, y: -0.8, z: 0.03} # cf_12 + - pose: {x: -0.5, y: -0.8, z: 0.03} # cf_13 + - pose: {x: -0.3, y: -0.8, z: 0.03} # cf_14 + - pose: {x: -0.1, y: -0.8, z: 0.03} # cf_15 + - pose: {x: 0.1, y: -0.8, z: 0.03} # cf_16 + - pose: {x: 0.3, y: -0.8, z: 0.03} # cf_17 + - pose: {x: 0.5, y: -0.8, z: 0.03} # cf_18 + - pose: {x: 0.7, y: -0.8, z: 0.03} # cf_19 + - pose: {x: 0.9, y: -0.8, z: 0.03} # cf_20 + +sim: + models: true # grippers are drawn only with the robot meshes + # The generator's map for this world (poles only: branches and platforms + # are not in the .bt). + map: /root/ros2_ws/src/hero_mrs_control_stack/hero_mrs_ros2/worlds/perch3_world.bt + # Off: a perch approach flies straight with obstacle avoidance off, and + # hero_sim stops a drone at a trunk at any altitude. Perch paths are now + # checked for clearance, so this could be turned on to catch a regression. + collision: false diff --git a/config/test_redeploy_1.yaml b/config/mrs/test_redeploy_1.yaml similarity index 100% rename from config/test_redeploy_1.yaml rename to config/mrs/test_redeploy_1.yaml diff --git a/config/test_redeploy_2.yaml b/config/mrs/test_redeploy_2.yaml similarity index 100% rename from config/test_redeploy_2.yaml rename to config/mrs/test_redeploy_2.yaml diff --git a/config/mrs/test_unperch.yaml b/config/mrs/test_unperch.yaml new file mode 100644 index 0000000..b05759b --- /dev/null +++ b/config/mrs/test_unperch.yaml @@ -0,0 +1,29 @@ +# test_deploy_2: ported from agilex_sitl hero_sitl/config/test_deploy_2.yaml. +# All keys, and the sim: block defaults, are documented in example.yaml. + +ground_robots: + - name: scout_1 + pose: {x: 0.0, y: 0.0, yaw: 0.0} + drive: omni + model: scout + - name: herover_1 + pose: {x: 0.0, y: 0.6, yaw: 0.0} + drive: omni + model: herover + # - name: herover_2 + # pose: {x: 0.0, y: -0.8, yaw: 0.0} + # drive: omni + # model: herover + # - name: herover_3 + # pose: {x: 0.0127, y: -1.25144, yaw: 0.0} + # drive: omni + # model: herover + +crazyflies: + - {pose: {x: 0.5, y: 0.090581, z: 0.01}, gripper: true} # cf_1 + - {pose: {x: 0.5, y: -0.090581, z: 0.01}, gripper: true} # cf_2 + # - {pose: {x: 0.113226, y: -0.090581, z: 0.23}, gripper: true} # cf_3 + # - {pose: {x: 0.113226, y: 0.090581, z: 0.23}, gripper: true} # cf_4 + +sim: + models: true diff --git a/config/v2_dock4.yaml b/config/mrs/v2_dock4.yaml similarity index 100% rename from config/v2_dock4.yaml rename to config/mrs/v2_dock4.yaml diff --git a/config/sim.rviz b/config/sim.rviz index 7675c63..66480f8 100644 --- a/config/sim.rviz +++ b/config/sim.rviz @@ -8,13 +8,10 @@ Panels: - /Grid1 - /Axes1 - /TF1 - - /TF1/Frames1 - - /Map1 - - /Map2 - - /Map3 - - /Map4 + - /Obstacles1 + - /MarkerArray2 Splitter Ratio: 0.5 - Tree Height: 437 + Tree Height: 1285 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -65,25 +62,21 @@ Visualization Manager: Enabled: true Frame Timeout: 15 Frames: - All Enabled: false + All Enabled: true cf_1: - Value: false + Value: true cf_2: - Value: false - cf_3: - Value: false - cf_4: - Value: false + Value: true herover_1: - Value: false + Value: true scout_1: - Value: false + Value: true world: Value: true Marker Scale: 1 Name: TF Show Arrows: false - Show Axes: true + Show Axes: false Show Names: true Tree: world: @@ -91,10 +84,6 @@ Visualization Manager: {} cf_2: {} - cf_3: - {} - cf_4: - {} herover_1: {} scout_1: @@ -119,7 +108,7 @@ Visualization Manager: Enabled: true Name: Grippers Namespaces: - {} + gripper: true Topic: Depth: 1 Durability Policy: Transient Local @@ -128,8 +117,8 @@ Visualization Manager: Value: /hero_sim/grippers Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray + Enabled: true + Name: Tether Namespaces: {} Topic: @@ -137,26 +126,25 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /swarm/agent_markers - Value: false + Value: /hero_sim/tether + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: MarkerArray + Name: Obstacles Namespaces: {} Topic: - Depth: 5 - Durability Policy: Volatile + Depth: 1 + Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /swarm/battery_levels + Value: /hero_sim/obstacles Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray Namespaces: - perching_positions: true - perching_positions_axis: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -164,6 +152,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /swarm/perching_markers Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Docks + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /swarm/dock_markers + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray @@ -171,7 +171,7 @@ Visualization Manager: map: true Topic: Depth: 5 - Durability Policy: Volatile + Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable Value: /occupied_cells_vis_array @@ -180,7 +180,7 @@ Visualization Manager: Enabled: true Name: Marker Namespaces: - comm_edges: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -469,6 +469,18 @@ Visualization Manager: Value: /cf_4/occupancy_grid_updates Use Timestamp: false Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupied_cells_vis_array + Value: false Enabled: true Global Options: Background Color: 90; 90; 90 @@ -515,25 +527,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 7.194181442260742 + Distance: 6.790585517883301 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.7366625070571899 - Y: 0.22696661949157715 - Z: 0.531701922416687 + X: -0.25788095593452454 + Y: 0.3019346594810486 + Z: 0.32738953828811646 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7697970271110535 + Pitch: 0.7347971200942993 Target Frame: Value: Orbit (rviz) - Yaw: 3.4754130840301514 + Yaw: 3.945415735244751 Saved: ~ Window Geometry: Displays: @@ -541,7 +553,7 @@ Window Geometry: Height: 1403 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015a00000546fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000001700000546000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000217000003460000000000000000fb0000000a0049006d00610067006501000002c70000030e00000000000000000000000100000102000005befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000017000005be000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fa0000003efc0100000002fb0000000800540069006d00650000000000000004fa000002d200fffffffb0000000800540069006d006501000000000000045000000000000000000000060c0000054600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000017000000546fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000001700000546000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000217000003460000000000000000fb0000000a0049006d00610067006501000002c70000030e00000000000000000000000100000102000005befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000017000005be000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fa0000003efc0100000002fb0000000800540069006d00650000000000000004fa000002d200fffffffb0000000800540069006d00650100000000000004500000000000000000000006ad0000054600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -550,6 +562,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1548 - X: 1012 + Width: 1709 + X: 851 Y: 0 diff --git a/config/tether/tether_demo.yaml b/config/tether/tether_demo.yaml new file mode 100644 index 0000000..a78b432 --- /dev/null +++ b/config/tether/tether_demo.yaml @@ -0,0 +1,38 @@ +# Tether demo: winch -> UGV pulley -> UAV ring -> object on the floor. +# Get the object into the goal circle. It slides on the floor, never lifts, +# and jams if pulled into the pillar: route it around. The pillar blocks the +# robots too, the drone at any height. +# +# bash start.sh tether_demo.yaml # sim + RViz +# +# Controls: /herover_1/cmd_vel or /herover_1/go_to, /cf_1/takeoff|go_to|land, +# /winch/cmd_rate (m/s, + out, - in). +# Feedback: /tether/object_pose, /tether/goal, /tether/taut, /tether/free_length, +# /tether/stuck, /tether/goal_reached, /winch/length, /hero_sim/obstacles. +# See README "Tether task". + +ground_robots: + - name: herover_1 + pose: {x: -1.5, y: -1.5, yaw: 0.0} + drive: omni + model: herover + +crazyflies: + - pose: {x: 0.0, y: 1.0, z: 0.03} # cf_1 + +tether: + winch: {x: -3.0, y: 0.0, z: 0.3} # m, where the tether leaves the drum + ugv: herover_1 # pulley on its deck (pose z) + uav: cf_1 # guide ring + object: {x: 0.5, y: -0.5} # start on the floor + object_size: 0.3 # m, cube + goal: {x: 2.5, y: 1.5, radius: 0.4} + slack: 3.0 # m of tether beyond taut at the start + max_reel_speed: 0.5 # m/s + +obstacles: # pillars: block the object and the robots + - {x: 1.5, y: 0.5, radius: 0.35} # right on the straight line to the goal + +sim: + map: null + collision: true diff --git a/config/two_ugvs.yaml b/config/two_ugvs.yaml new file mode 100644 index 0000000..821a7eb --- /dev/null +++ b/config/two_ugvs.yaml @@ -0,0 +1,33 @@ +# Two UGVs, no tether: the tether demo's floor and pillar, with a second +# herover and no drone. +# +# bash start.sh two_ugvs.yaml # sim + RViz +# +# Controls: /herover_/cmd_vel (Twist, body frame) or /herover_/go_to +# (crazyflie_interfaces/srv/GoTo). Feedback: /herover_/pose, +# /hero_sim/obstacles. See README "Driving the robots" and "Obstacles". +# +# Robot size: in collisions each robot is a circle of its `radius` (m), the +# only size the sim uses: against the pillar and against the other robot. +# Without `radius:` it's the model's default (hero_sim/config.py +# FOOTPRINT_RADII: herover 0.17, scout 0.42; no model 0.3). 0 turns its +# collisions off. The RViz mesh keeps its real size whatever you set here. + +ground_robots: + - name: herover_1 + pose: {x: -1.5, y: -1.5, yaw: 0.0} + drive: omni + model: herover + radius: 0.17 # m, the herover default + - name: herover_2 + pose: {x: -1.5, y: 1.5, yaw: 0.0} + drive: omni + model: herover + radius: 0.17 # m, the herover default + +obstacles: # pillars: block both robots + - {x: 1.5, y: 0.5, radius: 0.35} + +sim: + map: null + collision: true diff --git a/hero_sim/config.py b/hero_sim/config.py index ede833a..6f3f601 100644 --- a/hero_sim/config.py +++ b/hero_sim/config.py @@ -1,29 +1,36 @@ """Load and validate a hero_sim scenario config. No ROS imports at module level. -A scenario file has three sections (see config/example.yaml): +A scenario file has these sections (see config/example.yaml): ground_robots kinematic ground robots, one entry per robot crazyflies Crazyflies, named cf_1..cf_N in list order sim optional overrides of SIM_DEFAULTS + obstacles optional pillars that block robots and the tether object; they make + their own octomap, so a scenario has obstacles or sim.map, not both + tether optional winch, tether and object (hero_sim/tether.py) Unknown keys are rejected so typos fail loudly instead of being ignored. """ from dataclasses import dataclass +import math import os from pathlib import Path +from typing import NamedTuple import yaml SIM_DEFAULTS = { 'ground_rate': 50.0, # Hz, ground robot integration + /pose rate - 'ground_max_speed': 2.0, # m/s, clamp on |(linear.x, linear.y)| - 'ground_max_yaw_rate': 2.0, # rad/s, clamp on |angular.z| + 'ground_max_speed': 2.0, # m/s, clamp on |(linear.x, linear.y)| and go_to + 'ground_max_yaw_rate': 2.0, # rad/s, clamp on |angular.z| and go_to 'cf_rate': 10.0, # Hz, Crazyflie update + /pose rate 'cf_battery_drain': 0.025, # V/s while flying (starts at 4.2 V, floor 3.0 V) 'attach': 'auto', # sim_attach dock ride-along: auto | true | false (see attach_enabled) 'rviz': True, # open RViz with config/sim.rviz 'models': True, # robot meshes in RViz (crazyflies + ground robots with a model) 'map': 'icuas26_1', # octomap: maps/.bt, a path to a .bt, or null + 'collision': True, # stop robots at the map, pillars and each other (obstacles.py) + 'collision_z': (0.2, 2.0), # m, map heights counted as obstacles: trunks, not floor/canopy } # RViz meshes by model name, relative to models/. Crazyflies always use @@ -34,6 +41,26 @@ 'scout': 'scout/scout.glb', } +# Footprint radius (m) for map collisions, by model: the circle around the +# mesh seen from above. Ground robots without a model get +# DEFAULT_GROUND_RADIUS. Any robot can set its own with `radius:`. +FOOTPRINT_RADII = { + 'crazyflie': 0.1, + 'herover': 0.17, + 'scout': 0.42, +} +DEFAULT_GROUND_RADIUS = 0.3 +DEFAULT_PILLAR_HEIGHT = 2.0 # m + +# Height (m) a ground robot's pose reports, by model: on hardware the mocap +# markers sit on top, so the pose is up there and not on the floor. Measured +# as the top of the deck in the model's mesh. Ground robots without a model +# get 0. Any robot can set its own with `pose: {z: ...}`. +MARKER_HEIGHTS = { + 'herover': 0.095, + 'scout': 0.227, +} + DRIVES = ('omni', 'diff') CONFIG_ENV = 'HERO_SIM_CONFIG' DEFAULT_CONFIG = 'example.yaml' @@ -47,6 +74,8 @@ class GroundRobot: yaw: float drive: str model: str = None # key of MODEL_MESHES, or None for no mesh + radius: float = DEFAULT_GROUND_RADIUS # m, footprint for map collisions + z: float = 0.0 # m, height of the published pose (MARKER_HEIGHTS) @dataclass(frozen=True) @@ -56,6 +85,33 @@ class Crazyflie: y: float z: float gripper: bool = False # starts with an open gripper on top (see hero_sim/gripper.py) + radius: float = FOOTPRINT_RADII['crazyflie'] # m, footprint for map collisions + + +class Pillar(NamedTuple): + """One entry of a scenario's obstacles: list, in m.""" + x: float + y: float + radius: float + height: float = DEFAULT_PILLAR_HEIGHT # in RViz and the octomap; collisions ignore it + + +@dataclass(frozen=True) +class Tether: + """Winch -> pulley on a ground robot -> ring on a Crazyflie -> object on the floor.""" + winch: tuple # x, y, z (m): where the tether leaves the drum + ugv: str # ground robot carrying the first pulley (at its pose z) + uav: str # Crazyflie carrying the guide ring + object: tuple # x, y (m): start position on the floor + goal: tuple # x, y, radius (m) + object_size: float = 0.3 # m, cube edge + slack: float = 0.5 # m of tether beyond taut at the start + max_reel_speed: float = 0.5 # m/s, clamp on /winch/cmd_rate + + @property + def object_radius(self): + """m, the object's footprint against obstacles: half its edge.""" + return self.object_size / 2.0 @dataclass(frozen=True) @@ -63,6 +119,8 @@ class Scenario: ground_robots: tuple crazyflies: tuple sim: dict + tether: Tether = None + obstacles: tuple = () # Pillars, see hero_sim/obstacles.py def load(path): @@ -78,7 +136,7 @@ def load(path): def parse(raw): """Validate an already-loaded scenario mapping and return a Scenario.""" raw = {} if raw is None else raw - _check_keys(raw, ('ground_robots', 'crazyflies', 'sim'), 'top level') + _check_keys(raw, ('ground_robots', 'crazyflies', 'sim', 'obstacles', 'tether'), 'top level') ground = tuple(_ground_robot(i, r) for i, r in enumerate(_list(raw, 'ground_robots'))) names = [r.name for r in ground] @@ -103,8 +161,23 @@ def parse(raw): raise ValueError(f'sim.cf_battery_drain must be >= 0, got {sim["cf_battery_drain"]}') if not (isinstance(sim['attach'], bool) or sim['attach'] == 'auto'): raise ValueError(f'sim.attach must be auto, true or false, got {sim["attach"]!r}') - - return Scenario(ground, cfs, sim) + if not isinstance(sim['collision'], bool): + raise ValueError(f'sim.collision must be true or false, got {sim["collision"]!r}') + band = sim['collision_z'] + if not (isinstance(band, (list, tuple)) and len(band) == 2 + and float(band[0]) < float(band[1])): + raise ValueError(f'sim.collision_z must be [z_min, z_max], z_min < z_max, got {band!r}') + sim['collision_z'] = (float(band[0]), float(band[1])) + + pillars = tuple(_pillar(i, o) for i, o in enumerate(_list(raw, 'obstacles'))) + if pillars: + if sim_raw.get('map'): + raise ValueError('sim.map and obstacles: are both set; use one or the other ' + '(the obstacles make their own map)') + sim['map'] = None # instead of the default map + tether = (_tether(raw['tether'], ground, cfs, pillars) + if raw.get('tether') is not None else None) + return Scenario(ground, cfs, sim, tether, pillars) def attach_enabled(sim): @@ -141,15 +214,22 @@ def _check_keys(d, allowed, where): raise ValueError(f'{where}: unknown key(s) {unknown}; allowed: {sorted(allowed)}') -def _pose(entry, where, axes): +def _pose(entry, where, axes, **defaults): pose = entry.get('pose') or {} _check_keys(pose, axes, f'{where}.pose') - return [float(pose.get(a, 0.0)) for a in axes] + return [float(pose.get(a, defaults.get(a, 0.0))) for a in axes] + + +def _radius(entry, where, default): + radius = float(entry.get('radius', default)) + if radius < 0.0: + raise ValueError(f'{where}: radius must be >= 0, got {radius}') + return radius def _ground_robot(i, entry): where = f'ground_robots[{i}]' - _check_keys(entry, ('name', 'pose', 'drive', 'model'), where) + _check_keys(entry, ('name', 'pose', 'drive', 'model', 'radius'), where) name = entry.get('name') if not isinstance(name, str) or not name: raise ValueError(f'{where}: name is required') @@ -160,19 +240,78 @@ def _ground_robot(i, entry): if model is not None and model not in MODEL_MESHES: raise ValueError(f'{where} ({name}): model must be one of {sorted(MODEL_MESHES)} ' f'or left out, got {model!r}') - x, y, yaw = _pose(entry, f'{where} ({name})', ('x', 'y', 'yaw')) - return GroundRobot(name, x, y, yaw, drive, model) + x, y, z, yaw = _pose(entry, f'{where} ({name})', ('x', 'y', 'z', 'yaw'), + z=MARKER_HEIGHTS.get(model, 0.0)) + radius = _radius(entry, f'{where} ({name})', FOOTPRINT_RADII.get(model, DEFAULT_GROUND_RADIUS)) + return GroundRobot(name, x, y, yaw, drive, model, radius, z) def _crazyflie(i, entry): name = f'cf_{i + 1}' where = f'crazyflies[{i}] ({name})' - _check_keys(entry, ('pose', 'gripper'), where) + _check_keys(entry, ('pose', 'gripper', 'radius'), where) gripper = entry.get('gripper', False) if not isinstance(gripper, bool): raise ValueError(f'{where}: gripper must be true or false, got {gripper!r}') x, y, z = _pose(entry, where, ('x', 'y', 'z')) - return Crazyflie(name, x, y, z, gripper) + return Crazyflie(name, x, y, z, gripper, _radius(entry, where, FOOTPRINT_RADII['crazyflie'])) + + +def _point(value, where, axes, **defaults): + """A mapping of `axes` as a tuple of floats. Axes without a default are required.""" + value = value or {} + _check_keys(value, axes, where) + missing = [a for a in axes if a not in value and a not in defaults] + if missing: + raise ValueError(f'{where}: missing {missing}') + return tuple(float(value.get(a, defaults.get(a))) for a in axes) + + +def _pillar(i, entry): + where = f'obstacles[{i}]' + pillar = Pillar(*_point(entry, where, ('x', 'y', 'radius', 'height'), + height=DEFAULT_PILLAR_HEIGHT)) + for key in ('radius', 'height'): + if getattr(pillar, key) <= 0.0: + raise ValueError(f'{where}.{key} must be > 0, got {getattr(pillar, key)}') + return pillar + + +def _tether(entry, ground, cfs, pillars): + where = 'tether' + if isinstance(entry, dict) and 'obstacles' in entry: + raise ValueError(f'{where}.obstacles moved to the top-level obstacles: list, ' + 'which blocks the robots too') + _check_keys(entry, ('winch', 'ugv', 'uav', 'object', 'goal', 'object_size', + 'slack', 'max_reel_speed'), where) + ugv = entry.get('ugv', ground[0].name if ground else None) + if ugv not in {r.name for r in ground}: + raise ValueError(f'{where}.ugv must name a ground robot, got {ugv!r}') + uav = entry.get('uav', 'cf_1') + if uav not in {c.name for c in cfs}: + raise ValueError(f'{where}.uav must name a crazyflie (cf_1, ...), got {uav!r}') + tether = Tether( + winch=_point(entry.get('winch'), f'{where}.winch', ('x', 'y', 'z'), z=0.3), + ugv=ugv, + uav=uav, + object=_point(entry.get('object'), f'{where}.object', ('x', 'y')), + goal=_point(entry.get('goal'), f'{where}.goal', ('x', 'y', 'radius'), radius=0.4), + object_size=float(entry.get('object_size', Tether.object_size)), + slack=float(entry.get('slack', Tether.slack)), + max_reel_speed=float(entry.get('max_reel_speed', Tether.max_reel_speed)), + ) + for key in ('object_size', 'max_reel_speed'): + if getattr(tether, key) <= 0.0: + raise ValueError(f'{where}.{key} must be > 0, got {getattr(tether, key)}') + if tether.slack < 0.0: + raise ValueError(f'{where}.slack must be >= 0, got {tether.slack}') + if tether.goal[2] <= 0.0: + raise ValueError(f'{where}.goal.radius must be > 0, got {tether.goal[2]}') + ox, oy = tether.object + for i, p in enumerate(pillars): + if math.hypot(ox - p.x, oy - p.y) < p.radius + tether.object_radius: + raise ValueError(f'{where}.object starts inside obstacles[{i}]') + return tether # ---------------------------------------------------------------- resolution @@ -183,17 +322,45 @@ def share_dir(): return Path(get_package_share_directory('hero_sim')) +def find_config(arg, root): + """The scenario file `arg` names, under the config dir `root` (startup/_setup.sh does the same). + + Configs sit in root or in folders below it (config/tether/, config/mrs/): + tether_demo.yaml a bare filename, searched in root and every folder below. + Filenames are unique across folders; two matches raise + tether/tether_demo.yaml with a '/': a path if that file exists, else relative to root + """ + root = Path(root) + if '/' in arg: + for path in (Path(arg), root / arg): + if path.is_file(): + return path + raise FileNotFoundError(f'hero_sim config not found: {arg} (as a path or under {root})') + matches = sorted(p for p in root.rglob(arg) if p.is_file()) + if len(matches) > 1: + names = ', '.join(str(p.relative_to(root)) for p in matches) + raise ValueError(f'hero_sim config {arg} is in more than one folder: {names}; ' + 'name the folder too, e.g. tether/tether_demo.yaml. If a config ' + 'moved, the install may still hold its old copy: remove ' + 'build/hero_sim and install/hero_sim and rebuild') + if not matches: + raise FileNotFoundError(f'hero_sim config not found: {arg} (searched {root})') + return matches[0] + + def resolve_config(arg=''): """Turn a config argument into an existing file path. - Empty falls back to $HERO_SIM_CONFIG, then DEFAULT_CONFIG. A bare filename - is looked up in the package's config/ dir; anything with a '/' is a path. + Empty falls back to $HERO_SIM_CONFIG, then DEFAULT_CONFIG. Names are found + in the package's config/ dir and its folders (find_config); anything + else with a '/' is a path. """ arg = arg or os.environ.get(CONFIG_ENV, '') or DEFAULT_CONFIG - path = Path(arg) if '/' in arg else share_dir() / 'config' / arg - if not path.is_file(): - raise FileNotFoundError(f'hero_sim config not found: {path}') - return str(path.resolve()) + if Path(arg).is_absolute(): # a path: no package lookup, so no sourced ROS env needed + if not Path(arg).is_file(): + raise FileNotFoundError(f'hero_sim config not found: {arg}') + return str(Path(arg).resolve()) + return str(find_config(arg, share_dir() / 'config').resolve()) def resolve_map(value): diff --git a/hero_sim/crazyflie.py b/hero_sim/crazyflie.py index c5e54bd..46725a6 100644 --- a/hero_sim/crazyflie.py +++ b/hero_sim/crazyflie.py @@ -19,6 +19,10 @@ class SimCrazyflie: low-level cmd_position / cmd_full_state / cmd_velocity_world / cmd_vel_legacy, active for LOW_LEVEL_TIMEOUT after the last msg high-level takeoff / land / go_to, linear interpolation over the duration + + With an ObstacleMap (hero_sim/obstacles.py), every source's horizontal + motion stops where the footprint of `radius` would hit an obstacle; height + changes always go through. set_position teleports without a check. """ # Supervisor info bits (crazyflie_interfaces/msg/Status) @@ -36,15 +40,18 @@ class SimCrazyflie: BATTERY_LOW = 3.3 # V, below this pm_state reports LOW_POWER def __init__(self, node: Node, name, initial_position=(0.0, 0.0, 0.0), dt=0.1, - battery_drain=0.025, frame='world'): + battery_drain=0.025, frame='world', radius=0.1, obstacles=None): self.node = node self.name = name self.frame = frame self.dt = dt self.battery_drain = battery_drain # V/s while flying + self.radius = radius # m, footprint checked against `obstacles` + self.obstacles = obstacles # ObstacleMap, or None for no map collisions self.position = np.array(initial_position, dtype=float) self.yaw = 0.0 + self._checked_xy = tuple(self.position[:2]) # x, y the obstacle check last let through # High-level motion (linear interpolation start -> goal) self.start_position = self.position.copy() @@ -120,6 +127,7 @@ def update(self): pass # position already set by the low-level callbacks elif self.motion_active: self._update_high_level(dt) + self._keep_clear() if self.flying: self.battery_voltage = max(self.battery_voltage - self.battery_drain * dt, @@ -153,6 +161,15 @@ def _update_high_level(self, dt): if self.position[2] < 0.05: self.flying = False + def _keep_clear(self): + """Stop x, y where the straight path from the last checked point meets an obstacle.""" + if self.obstacles is None: + return + x, y = self.obstacles.move(*self._checked_xy, self.position[0], self.position[1], + self.radius) + self.position[0], self.position[1] = x, y + self._checked_xy = (x, y) + def _publish_pose(self): stamp = self.node.get_clock().now().to_msg() x, y, z = (float(v) for v in self.position) @@ -280,6 +297,7 @@ def _upload_trajectory_cb(self, request, response): def _set_position_cb(self, request, response): self.position = np.array([request.goal.x, request.goal.y, request.goal.z]) self.yaw = request.yaw + self._checked_xy = (request.goal.x, request.goal.y) # a teleport skips the path check self.goal = self.position.copy() self.start_position = self.position.copy() self.motion_active = False @@ -290,6 +308,7 @@ def _set_position_cb(self, request, response): # ------------------------------------------------------------ cmd topics def _mark_low_level(self): + self._keep_clear() self.goal = self.position.copy() self.motion_active = False self.last_low_level_time = self.node.get_clock().now() diff --git a/hero_sim/crazyflie_server.py b/hero_sim/crazyflie_server.py index 7d6620a..0e3fab8 100644 --- a/hero_sim/crazyflie_server.py +++ b/hero_sim/crazyflie_server.py @@ -10,7 +10,7 @@ from rclpy.node import Node from tf2_ros import TransformBroadcaster -from hero_sim import config +from hero_sim import config, obstacles from hero_sim.crazyflie import SimCrazyflie # (service type, /all/, SimCrazyflie callback) @@ -34,12 +34,20 @@ def __init__(self): config_path = config.resolve_config(self.get_parameter('config_path').value) scenario = config.load(config_path) dt = 1.0 / scenario.sim['cf_rate'] + obstacle_map = obstacles.load(scenario.sim, [cf.radius for cf in scenario.crazyflies], + scenario.obstacles) self.drones = { cf.name: SimCrazyflie(self, cf.name, (cf.x, cf.y, cf.z), dt=dt, - battery_drain=scenario.sim['cf_battery_drain']) + battery_drain=scenario.sim['cf_battery_drain'], + radius=cf.radius, obstacles=obstacle_map) for cf in scenario.crazyflies } + if obstacle_map is not None: + for cf in scenario.crazyflies: + if obstacle_map.clearance(cf.x, cf.y) < cf.radius: + self.get_logger().warn(f'{cf.name} starts overlapping an obstacle; ' + 'it can only move away from it') for srv_type, name, method in _BROADCASTS: self.create_service(srv_type, f'/all/{name}', partial(self._broadcast, name, method)) @@ -48,9 +56,10 @@ def __init__(self): self.create_timer(dt, self._update_all) self.create_timer(STATUS_PERIOD, self._publish_status_all) + collisions = 'off' if obstacle_map is None else 'on' self.get_logger().info( - f'{len(self.drones)} crazyflies at {scenario.sim["cf_rate"]:g} Hz from {config_path}: ' - f'{list(self.drones)}') + f'{len(self.drones)} crazyflies at {scenario.sim["cf_rate"]:g} Hz from {config_path}, ' + f'collisions {collisions}: {list(self.drones)}') def _update_all(self): transforms = [drone.update() for drone in self.drones.values()] diff --git a/hero_sim/ground_robots.py b/hero_sim/ground_robots.py index 791bb9b..396c75e 100644 --- a/hero_sim/ground_robots.py +++ b/hero_sim/ground_robots.py @@ -2,29 +2,54 @@ cmd_vel is a body-frame Twist held until the next message. omni robots honor linear.x, linear.y and angular.z; diff robots ignore linear.y. Poses are -published in 'world' at z=0, along with a world -> transform. +published in 'world' at the robot's pose z (its marker height, see +config.MARKER_HEIGHTS), along with a world -> transform. + +//go_to (crazyflie_interfaces/srv/GoTo, as for the Crazyflies) drives +to goal x, y and yaw in `duration`, or slower to stay within the speed limits +(kinematics.GroundGoTo). relative makes x, y an offset in the world frame; +goal.z and group_mask are ignored. Any cmd_vel message cancels it. + +With sim.collision on, a robot stops where its footprint would hit the map, a +pillar or another ground robot (hero_sim/obstacles.py), and resumes when +cmd_vel points away. A blocked go_to waits where it stopped and carries on +once the way is clear. The robots move one after another each tick, each +against the others' current positions, so they never end up overlapping. + +This node also draws the scenario's pillars (its obstacles: list), latched on +/hero_sim/obstacles: one CYLINDER per pillar, at its x, y, scale.x = diameter, +scale.z = height. (octomap.launch.py serves them as an octomap too.) """ +import math + +from crazyflie_interfaces.srv import GoTo from geometry_msgs.msg import PoseStamped, TransformStamped, Twist import rclpy from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from tf2_ros import TransformBroadcaster +from visualization_msgs.msg import Marker, MarkerArray -from hero_sim import config -from hero_sim.kinematics import integrate_ground, limit_ground_cmd, yaw_to_quat +from hero_sim import config, obstacles +from hero_sim.kinematics import GroundGoTo, integrate_ground, limit_ground_cmd, yaw_to_quat FRAME = 'world' +OBSTACLE_TOPIC = '/hero_sim/obstacles' class _Robot: - __slots__ = ('name', 'drive', 'x', 'y', 'yaw', 'vx', 'vy', 'wz', 'pose_pub') + __slots__ = ('name', 'drive', 'radius', 'x', 'y', 'z', 'yaw', 'vx', 'vy', 'wz', 'pose_pub', + 'go_to', 'go_to_t') def __init__(self, spec, pose_pub): self.name = spec.name self.drive = spec.drive - self.x, self.y, self.yaw = spec.x, spec.y, spec.yaw + self.radius = spec.radius + self.x, self.y, self.z, self.yaw = spec.x, spec.y, spec.z, spec.yaw self.vx = self.vy = self.wz = 0.0 self.pose_pub = pose_pub + self.go_to = None # GroundGoTo while one runs + self.go_to_t = 0.0 # s into it class GroundRobotSim(Node): @@ -46,32 +71,110 @@ def __init__(self): robot = _Robot(spec, self.create_publisher(PoseStamped, f'/{spec.name}/pose', qos)) self.create_subscription(Twist, f'/{spec.name}/cmd_vel', lambda msg, r=robot: self._on_cmd_vel(r, msg), qos) + self.create_service(GoTo, f'/{spec.name}/go_to', + lambda req, resp, r=robot: self._on_go_to(r, req, resp)) self._robots.append(robot) + self._obstacles = obstacles.load(sim, [r.radius for r in self._robots], + scenario.obstacles) + if self._obstacles is not None: + for r in self._robots: + if self._obstacles.clearance(r.x, r.y) < r.radius: + self.get_logger().warn(f'{r.name} starts overlapping an obstacle; ' + 'it can only move away from it') + for i, a in enumerate(self._robots): + for b in self._robots[i + 1:]: + if (a.radius > 0.0 and b.radius > 0.0 + and math.hypot(a.x - b.x, a.y - b.y) < a.radius + b.radius): + self.get_logger().warn(f'{a.name} and {b.name} start overlapping; ' + 'they can only move apart') + self._publish_pillars(scenario.obstacles) + self._tf = TransformBroadcaster(self) self.create_timer(self._dt, self._tick) summary = ', '.join(f'{r.name}({r.drive})' for r in self._robots) or 'none' + collisions = 'off' if self._obstacles is None else 'on' self.get_logger().info( f'ground robots at {sim["ground_rate"]:g} Hz, max {self._max_speed:g} m/s, ' - f'{self._max_yaw_rate:g} rad/s: {summary}') + f'{self._max_yaw_rate:g} rad/s, collisions {collisions}, ' + f'{len(scenario.obstacles)} pillar(s): {summary}') + + def _publish_pillars(self, pillars): + latched = QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + self._pillar_pub = self.create_publisher(MarkerArray, OBSTACLE_TOPIC, latched) + clear = Marker() + clear.header.frame_id = FRAME + clear.action = Marker.DELETEALL + markers = [clear] + for i, p in enumerate(pillars): # config.Pillar; the height is drawn only + m = Marker() + m.header.frame_id = FRAME + m.ns = 'obstacles' + m.id = i + m.type = Marker.CYLINDER + m.action = Marker.ADD + m.pose.position.x, m.pose.position.y, m.pose.position.z = p.x, p.y, p.height / 2.0 + m.pose.orientation.w = 1.0 + m.scale.x = m.scale.y = 2.0 * p.radius + m.scale.z = p.height + m.color.r, m.color.g, m.color.b, m.color.a = 0.55, 0.45, 0.4, 1.0 + markers.append(m) + self._pillar_pub.publish(MarkerArray(markers=markers)) def _on_cmd_vel(self, robot, msg): + robot.go_to = None robot.vx, robot.vy, robot.wz = limit_ground_cmd( msg.linear.x, msg.linear.y, msg.angular.z, robot.drive, self._max_speed, self._max_yaw_rate) + def _on_go_to(self, robot, request, response): + x, y = request.goal.x, request.goal.y + if request.relative: + x, y = robot.x + x, robot.y + y + duration = request.duration.sec + request.duration.nanosec * 1e-9 + robot.go_to = GroundGoTo((robot.x, robot.y, robot.yaw), (x, y, request.yaw), robot.drive, + duration, self._max_speed, self._max_yaw_rate) + robot.go_to_t = 0.0 + robot.vx = robot.vy = robot.wz = 0.0 + total = robot.go_to.total + slower = f' (asked {duration:.2f}s, held to the speed limits)' if total > duration else '' + self.get_logger().info( + f'[{robot.name}] GO_TO ({x:.2f}, {y:.2f}) yaw={request.yaw:.2f} ' + f'in {total:.2f}s{slower}') + return response + + def _others(self, robot): + """The other ground robots, as circles (x, y, radius) for `robot` to keep clear of.""" + return [(o.x, o.y, o.radius) for o in self._robots if o is not robot and o.radius > 0.0] + def _tick(self): stamp = self.get_clock().now().to_msg() transforms = [] for r in self._robots: - r.x, r.y, r.yaw = integrate_ground(r.x, r.y, r.yaw, r.vx, r.vy, r.wz, self._dt) + if r.go_to is not None: + t = r.go_to_t + self._dt + x, y, yaw = r.go_to.at(t) + else: + x, y, yaw = integrate_ground(r.x, r.y, r.yaw, r.vx, r.vy, r.wz, self._dt) + wanted = (x, y) + if self._obstacles is not None: + x, y = self._obstacles.move(r.x, r.y, x, y, r.radius, self._others(r)) + if r.go_to is None: + r.yaw = yaw + elif (x, y) == wanted: + r.go_to_t, r.yaw = t, yaw + if t >= r.go_to.total: + r.go_to = None + # else blocked: the go_to waits here, its clock stopped, until the way clears + r.x, r.y = x, y qx, qy, qz, qw = yaw_to_quat(r.yaw) pose = PoseStamped() pose.header.stamp = stamp pose.header.frame_id = FRAME - pose.pose.position.x, pose.pose.position.y = r.x, r.y + pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = r.x, r.y, r.z o = pose.pose.orientation o.x, o.y, o.z, o.w = qx, qy, qz, qw r.pose_pub.publish(pose) @@ -80,7 +183,8 @@ def _tick(self): t.header.stamp = stamp t.header.frame_id = FRAME t.child_frame_id = r.name - t.transform.translation.x, t.transform.translation.y = r.x, r.y + t.transform.translation.x, t.transform.translation.y, t.transform.translation.z = ( + r.x, r.y, r.z) t.transform.rotation = o transforms.append(t) if transforms: diff --git a/hero_sim/kinematics.py b/hero_sim/kinematics.py index 8089049..9069775 100644 --- a/hero_sim/kinematics.py +++ b/hero_sim/kinematics.py @@ -40,3 +40,48 @@ def integrate_ground(x, y, yaw, vx, vy, wz, dt): x += (vx * c - vy * s) * dt y += (vx * s + vy * c) * dt return x, y, wrap_angle(yaw + wz * dt) + + +class GroundGoTo: + """A ground robot's go_to: from `start` to `goal` (x, y, yaw), within the speed limits. + + omni drives straight while turning to the goal yaw. diff turns to face the + goal, drives straight, then turns to the goal yaw (closer than MIN_DRIVE + it only turns). The move takes `duration` s, or longer if that would + exceed max_speed or max_yaw_rate: then all of its phases stretch alike. + """ + + MIN_DRIVE = 0.01 # m + + def __init__(self, start, goal, drive, duration, max_speed, max_yaw_rate): + x0, y0, yaw0 = start + x1, y1, yaw1 = goal + dx, dy = x1 - x0, y1 - y0 + dist = math.hypot(dx, dy) + # Phases (shortest time, start pose, change in x, y, yaw), in order. + if drive == 'diff' and dist >= self.MIN_DRIVE: + heading = math.atan2(dy, dx) + turn_in, turn_out = wrap_angle(heading - yaw0), wrap_angle(yaw1 - heading) + phases = [ + (abs(turn_in) / max_yaw_rate, (x0, y0, yaw0), (0.0, 0.0, turn_in)), + (dist / max_speed, (x0, y0, heading), (dx, dy, 0.0)), + (abs(turn_out) / max_yaw_rate, (x1, y1, heading), (0.0, 0.0, turn_out)), + ] + else: + turn = wrap_angle(yaw1 - yaw0) + shortest = max(dist / max_speed, abs(turn) / max_yaw_rate) + phases = [(shortest, (x0, y0, yaw0), (dx, dy, turn))] + shortest = sum(p[0] for p in phases) + self.total = max(duration, shortest) # s + stretch = self.total / shortest if shortest > 0.0 else 0.0 + self._phases = [(t * stretch, pose, delta) for t, pose, delta in phases] + self.goal = (x1, y1, wrap_angle(yaw1)) + + def at(self, t): + """Pose (x, y, yaw) t s into the move; the goal from `total` on.""" + for span, (x, y, yaw), (dx, dy, dyaw) in self._phases: + if t < span: + a = t / span + return x + a * dx, y + a * dy, wrap_angle(yaw + a * dyaw) + t -= span + return self.goal diff --git a/hero_sim/obstacles.py b/hero_sim/obstacles.py new file mode 100644 index 0000000..6409288 --- /dev/null +++ b/hero_sim/obstacles.py @@ -0,0 +1,237 @@ +"""2D collisions: keep footprints out of the map, the pillars and each other. No ROS imports. + +Two kinds of fixed obstacle, both in one ObstacleMap: + + map the scenario's octomap (.bt), flattened once at startup: every + occupied voxel overlapping the height band sim.collision_z is an + obstacle, seen from above. The default band leaves out the floor + slab, ceilings and the tree canopies the drones perch in, and keeps + trunks, posts and walls. + pillars the scenario's top-level obstacles: list, circles (x, y, radius) + kept exact rather than gridded. A scenario has these or a map, not + both. For RViz, write_pillar_map turns them into an octomap. + +Moving robots are circles too, passed to each move as `others` +(hero_sim/ground_robots.py does that for the ground robots). + +Every robot is a disc of its footprint radius (config.FOOTPRINT_RADII), and so +is the tether object (hero_sim/tether.py). A move is checked along its +straight path, so fast robots can't tunnel through thin obstacles, and stops at +the last free point before the disc would touch one. Height is ignored: a +drone is blocked by a trunk or a pillar at any altitude. + +Map lookups are O(1): ObstacleMap precomputes each cell's distance to the +nearest map obstacle, up to the largest radius it will be asked about. +""" +import math +from pathlib import Path + +import numpy as np + +from hero_sim import config + +_TREE_DEPTH = 16 +_KEY_ORIGIN = 1 << (_TREE_DEPTH - 1) # octomap key of coordinate 0 +PILLAR_RES = 0.05 # m, cell size without a map; move() samples paths every half cell + + +def read_bt(path): + """Occupied leaves of an octomap .bt file. + + Returns (res, boxes): the voxel size in m, and an int array of rows + (kx, ky, kz, size) giving each occupied leaf's min corner in octomap keys + and its edge length in voxels (pruned leaves span several). + """ + with open(path, 'rb') as f: + if not f.readline().startswith(b'# Octomap OcTree binary file'): + raise ValueError(f'{path}: not an octomap .bt file') + header = {} + for line in f: + line = line.decode().strip() + if line == 'data': + break + if line and not line.startswith('#'): + key, value = line.split(None, 1) + header[key] = value + data = f.read() + if header.get('id') != 'OcTree': + raise ValueError(f'{path}: expected an OcTree, got {header.get("id")!r}') + + boxes = [] + pos = 0 + + def read_node(kx, ky, kz, size): + # 2 bits per child, children 0-7 in order: 01 free leaf, 10 occupied + # leaf, 11 inner node (read next, depth first). Child bit 0 is +x, + # bit 1 +y, bit 2 +z. + nonlocal pos + bits = data[pos] | data[pos + 1] << 8 + pos += 2 + half = size // 2 + inner = [] + for i in range(8): + kind = bits >> 2 * i & 3 + if kind >= 2: + child = (kx + half * (i & 1), ky + half * (i >> 1 & 1), kz + half * (i >> 2), half) + (boxes if kind == 2 else inner).append(child) + for child in inner: + read_node(*child) + + read_node(0, 0, 0, 1 << _TREE_DEPTH) + if pos != len(data): + raise ValueError(f'{path}: {len(data) - pos} trailing bytes after the tree') + return float(header['res']), np.array(boxes, dtype=np.int64).reshape(-1, 4) + + +class ObstacleMap: + """Obstacle cells on a 2D grid plus circular pillars, with the clearance to the nearest one.""" + + def __init__(self, occupied, x0, y0, res, max_radius, circles=()): + """occupied: bool array [row = y, col = x] with cell (0, 0) at (x0, y0). + + circles: pillars (x, y, radius[, height]), in m. Height is ignored. + """ + self.res = res + self.max_radius = max_radius + self.circles = tuple(tuple(float(v) for v in c[:3]) for c in circles) # drop any height + # Pad by the farthest reach so robots just outside the obstacles' extent are covered. + n = math.ceil(max_radius / res + 0.5) + self.x0, self.y0 = x0 - n * res, y0 - n * res + rows, cols = np.nonzero(occupied) + self._clearance = np.full((occupied.shape[0] + 2 * n, occupied.shape[1] + 2 * n), np.inf) + # Distance from a cell's centre to the nearest point of an obstacle cell + # (di, dj) cells away. Nearest last, so it wins where reaches overlap. + reach = sorted( + ((math.hypot(max(abs(di) - 0.5, 0.0), max(abs(dj) - 0.5, 0.0)) * res, di, dj) + for di in range(-n, n + 1) for dj in range(-n, n + 1)), reverse=True) + for d, di, dj in reach: + if d < max_radius: + self._clearance[rows + n + di, cols + n + dj] = d + + @classmethod + def from_bt(cls, path, z_min, z_max, max_radius, circles=()): + """Flatten the occupied voxels of a .bt that overlap z_min..z_max (m), plus `circles`.""" + res, boxes = read_bt(path) + z_lo = (boxes[:, 2] - _KEY_ORIGIN) * res + z_hi = z_lo + boxes[:, 3] * res + # Extruded walls repeat the same footprint on every layer: keep one. + boxes = np.unique(boxes[(z_hi > z_min) & (z_lo < z_max)][:, [0, 1, 3]], axis=0) + if not len(boxes): + return cls(np.zeros((0, 0), bool), 0.0, 0.0, res, max_radius, circles) + kx0, ky0 = boxes[:, :2].min(axis=0) + kx1, ky1 = (boxes[:, :2] + boxes[:, 2:]).max(axis=0) + occupied = np.zeros((ky1 - ky0, kx1 - kx0), bool) + for kx, ky, size in boxes - (kx0, ky0, 0): + occupied[ky:ky + size, kx:kx + size] = True + return cls(occupied, (kx0 - _KEY_ORIGIN) * res, (ky0 - _KEY_ORIGIN) * res, res, + max_radius, circles) + + def clearance(self, x, y, others=()): + """Distance (m) from (x, y) to the nearest obstacle; inf if beyond max_radius. + + others: more circles (x, y, radius) to count, such as other robots. + """ + best = math.inf + row = math.floor((y - self.y0) / self.res) + col = math.floor((x - self.x0) / self.res) + if 0 <= row < self._clearance.shape[0] and 0 <= col < self._clearance.shape[1]: + best = float(self._clearance[row, col]) + for cx, cy, r in (*self.circles, *others): + best = min(best, max(math.hypot(x - cx, y - cy) - r, 0.0)) + return best if best < self.max_radius else math.inf + + def move(self, x0, y0, x1, y1, radius, others=()): + """Where a disc of `radius` moving straight from (x0, y0) to (x1, y1) ends up. + + Samples the path every half cell and stops at the last sample before + the disc would touch an obstacle or one of `others` (circles x, y, + radius). A disc that already overlaps one (spawned or teleported + there) may move, just not deeper into it. Radius 0 never collides. + """ + if radius > self.max_radius: + raise ValueError(f'radius {radius} m is over max_radius {self.max_radius} m') + steps = int(math.hypot(x1 - x0, y1 - y0) / (0.5 * self.res)) + 1 + x, y = x0, y0 + clear = self.clearance(x, y, others) + for i in range(1, steps + 1): + nx, ny = x0 + (x1 - x0) * i / steps, y0 + (y1 - y0) * i / steps + next_clear = self.clearance(nx, ny, others) + if next_clear < radius and next_clear < clear: + return x, y + x, y, clear = nx, ny, next_clear + return x1, y1 + + +def pillar_voxels(pillars, res=PILLAR_RES): + """Octomap keys (kx, ky, kz) of the voxels that fill config.Pillars. + + A voxel is in a pillar when its center is inside the pillar's circle, from + the floor (z = 0, no ground plane) up to the pillar's height. + """ + keys = set() + for x, y, r, height in pillars: + layers = range(max(1, round(height / res))) + for kx in range(math.floor((x - r) / res), math.floor((x + r) / res) + 1): + for ky in range(math.floor((y - r) / res), math.floor((y + r) / res) + 1): + if math.hypot((kx + 0.5) * res - x, (ky + 0.5) * res - y) <= r: + keys.update((kx + _KEY_ORIGIN, ky + _KEY_ORIGIN, kz + _KEY_ORIGIN) + for kz in layers) + return keys + + +def write_bt(path, voxels, res): + """Write occupied voxels (octomap keys) as an octomap .bt file; read_bt reads it back. + + Every voxel is a leaf at full depth (nothing pruned), the rest is unknown. + """ + if not voxels: + raise ValueError('no voxels to write') + data = bytearray() + nodes = 1 # the root; the header's size must match what octomap reads + + def write_node(keys, kx, ky, kz, size): + # The read_node encoding: 2 bits per child, 10 occupied leaf, 11 inner node. + nonlocal nodes + half = size // 2 + children = [[] for _ in range(8)] + for key in keys: + i = (key[0] >= kx + half) | (key[1] >= ky + half) << 1 | (key[2] >= kz + half) << 2 + children[i].append(key) + kind = 2 if half == 1 else 3 + bits = sum(kind << 2 * i for i, child in enumerate(children) if child) + data.extend((bits & 0xFF, bits >> 8)) + nodes += sum(1 for child in children if child) + if kind == 3: + for i, child in enumerate(children): + if child: + write_node(child, kx + half * (i & 1), ky + half * (i >> 1 & 1), + kz + half * (i >> 2), half) + + write_node(sorted(voxels), 0, 0, 0, 1 << _TREE_DEPTH) + with open(path, 'wb') as f: + f.write(b'# Octomap OcTree binary file\n' + b'# written by hero_sim from a scenario\'s obstacles\n' + + f'id OcTree\nsize {nodes}\nres {res}\ndata\n'.encode() + bytes(data)) + + +def write_pillar_map(path, pillars, res=PILLAR_RES): + """Write config.Pillars as an octomap .bt at `path` (dirs made as needed). Returns path.""" + Path(path).parent.mkdir(parents=True, exist_ok=True) + write_bt(path, pillar_voxels(pillars, res), res) + return path + + +def load(sim, radii, pillars=()): + """The scenario's ObstacleMap for robots of these radii, or None when collisions are off. + + pillars: the scenario's obstacles (config.Pillar). None when + sim.collision is off or there are no robots. Without a map or pillars the + map is empty, but still checks the `others` passed to move(). + """ + if not sim['collision'] or not radii: + return None + bt_file = config.resolve_map(sim['map']) + if bt_file is None: + return ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, max(radii), pillars) + return ObstacleMap.from_bt(bt_file, *sim['collision_z'], max_radius=max(radii), + circles=pillars) diff --git a/hero_sim/robot_models.py b/hero_sim/robot_models.py index 00d8007..1912192 100644 --- a/hero_sim/robot_models.py +++ b/hero_sim/robot_models.py @@ -15,8 +15,8 @@ TOPIC = '/hero_sim/robot_models' -def mesh_marker(frame, model, marker_id): - """Mesh marker for `model` drawn at the origin of TF frame `frame`.""" +def mesh_marker(frame, model, marker_id, z=0.0): + """Mesh marker for `model` drawn at height `z` in TF frame `frame`.""" m = Marker() m.header.frame_id = frame # stamp 0: RViz uses the latest transform m.ns = model @@ -26,6 +26,7 @@ def mesh_marker(frame, model, marker_id): m.mesh_resource = f'package://hero_sim/models/{config.MODEL_MESHES[model]}' m.mesh_use_embedded_materials = True # color left at 0,0,0,0 so RViz doesn't tint m.frame_locked = True + m.pose.position.z = z m.pose.orientation.w = 1.0 m.scale.x = m.scale.y = m.scale.z = 1.0 return m @@ -34,7 +35,8 @@ def mesh_marker(frame, model, marker_id): def scenario_markers(scenario): markers = [mesh_marker(cf.name, 'crazyflie', i) for i, cf in enumerate(scenario.crazyflies)] - markers += [mesh_marker(r.name, r.model, i) + # A ground robot's frame is at its marker height: drop the mesh back to the floor. + markers += [mesh_marker(r.name, r.model, i, -r.z) for i, r in enumerate(scenario.ground_robots) if r.model] return markers diff --git a/hero_sim/tether.py b/hero_sim/tether.py new file mode 100644 index 0000000..8e24e4c --- /dev/null +++ b/hero_sim/tether.py @@ -0,0 +1,77 @@ +"""Tether model: a winch drags an object across the floor. No ROS imports. + +The tether runs from a fixed winch, through a pulley on a ground robot (UGV), +through a guide ring on a Crazyflie (UAV), down to an object on the floor: + + W (winch) -> G (UGV pulley) -> A (UAV ring) -> O (object) + +The winch pays out a total length L. What is left after the two moving +pulleys is the free length below the UAV: + + l = L - |W - G| - |G - A| + +Quasi-static (no swing), and the robots never feel the tether: + + slack |A - O| < l: the object stays put. + taut the object slides along the floor toward the point under the UAV + until |A - O| = l. + lifted never: the object is too heavy, so once it sits under the UAV the + winch slips (pays out) instead. + jammed pulled into an obstacle (a pillar or the map, hero_sim/obstacles.py), + the object stops where it touches it and the winch slips, until a + pull away from the obstacle frees it. The robots stop at the same + obstacles; the tether line itself passes through them. + +The node in hero_sim/tether_sim.py runs this for the scenario's tether: section. +""" +import math +from typing import NamedTuple + + +class State(NamedTuple): + length: float # m, paid out by the winch (grows when it slips) + obj: tuple # x, y (m) of the object on the floor + free: float # m, tether from the UAV ring to the object + taut: bool + stuck: bool # jammed against an obstacle + + +def initial_length(winch, ugv, uav, obj, attach_z, slack): + """Tether paid out with the robots where they are, leaving `slack` m to spare.""" + return (math.dist(winch, ugv) + math.dist(ugv, uav) + + math.dist(uav, (*obj, attach_z)) + slack) + + +def reached(obj, goal): + """Whether the object's center is inside the goal circle (x, y, radius).""" + return math.hypot(obj[0] - goal[0], obj[1] - goal[1]) <= goal[2] + + +def step(length, winch, ugv, uav, obj, attach_z, obstacles=None, radius=0.0): + """One quasi-static update of the tether and the object. Returns a State. + + length: tether paid out by the winch (m). winch, ugv, uav: (x, y, z) the + tether runs through, in order. obj: object (x, y) on the floor, tied at + height attach_z, with footprint `radius` against `obstacles`, an + obstacles.ObstacleMap or None for none. + """ + routed = math.dist(winch, ugv) + math.dist(ugv, uav) + dz = max(uav[2] - attach_z, 0.0) + # The object can't leave the floor: the winch slips rather than lift it. + length = max(length, routed + dz) + free = length - routed + reach = math.sqrt(max(free * free - dz * dz, 0.0)) # horizontal, from under the UAV + dx, dy = obj[0] - uav[0], obj[1] - uav[1] + h = math.hypot(dx, dy) + if h <= reach: + return State(length, tuple(obj), free, h > reach - 1e-3, False) + # Taut: slide toward the point under the UAV. + target = (uav[0] + dx * reach / h, uav[1] + dy * reach / h) + if obstacles is None: + return State(length, target, free, True, False) + contact = obstacles.move(obj[0], obj[1], target[0], target[1], radius) + if contact == target: + return State(length, target, free, True, False) + # Jammed against an obstacle: the object stops there and the winch slips. + free = math.dist(uav, (*contact, attach_z)) + return State(routed + free, contact, free, True, True) diff --git a/hero_sim/tether_sim.py b/hero_sim/tether_sim.py new file mode 100644 index 0000000..32c5b90 --- /dev/null +++ b/hero_sim/tether_sim.py @@ -0,0 +1,210 @@ +"""Winch, tether and object for the scenario's tether: section (model in hero_sim/tether.py). + +sim.launch.py starts this node only when the scenario has a tether: section. +It follows the UGV's and UAV's /pose and steps the model at sim.ground_rate. +With sim.collision on, the object stops at the scenario's pillars and map +obstacles (hero_sim/obstacles.py); ground_robot_sim draws the pillars. + +Topics: + /winch/cmd_rate std_msgs/Float64 in, m/s: + pays out, - reels in. Held until + the next msg, clamped to tether.max_reel_speed + /winch/length std_msgs/Float64 L, tether paid out + /tether/free_length std_msgs/Float64 l, from the UAV ring to the object + /tether/taut std_msgs/Bool + /tether/stuck std_msgs/Bool object jammed against an obstacle + /tether/object_pose geometry_msgs/PoseStamped object center + /tether/goal geometry_msgs/PoseStamped goal center (latched) + /tether/goal_reached std_msgs/Bool object center inside the goal circle + /hero_sim/tether visualization_msgs/MarkerArray +""" +from geometry_msgs.msg import Point, PoseStamped +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import Bool, Float64 +from visualization_msgs.msg import Marker, MarkerArray + +from hero_sim import config, obstacles, tether + +FRAME = 'world' +MARKER_TOPIC = '/hero_sim/tether' + + +def _color(m, r, g, b, a=1.0): + m.color.r, m.color.g, m.color.b, m.color.a = r, g, b, a + + +class TetherSim(Node): + + def __init__(self): + super().__init__('tether_sim') + self.declare_parameter('config_path', '') + scenario = config.load(config.resolve_config(self.get_parameter('config_path').value)) + if scenario.tether is None: + raise RuntimeError('scenario has no tether: section') + t = self._t = scenario.tether + self._dt = 1.0 / scenario.sim['ground_rate'] + # The winch never changes: resend it once a second, not every tick. + self._static_every = max(1, round(scenario.sim['ground_rate'])) + self._ticks = 0 + + self._attach_z = t.object_size / 2.0 + self._length = None # set from the first poses: taut + t.slack + self._state = None + self._reached = False + self._rate = 0.0 + self._ugv = self._uav = None + self._obstacles = obstacles.load(scenario.sim, [t.object_radius], scenario.obstacles) + if (self._obstacles is not None + and self._obstacles.clearance(*t.object) < t.object_radius): + self.get_logger().warn('the object starts overlapping an obstacle; ' + 'it can only be pulled away from it') + + self.create_subscription(PoseStamped, f'/{t.ugv}/pose', self._on_ugv, 10) + self.create_subscription(PoseStamped, f'/{t.uav}/pose', self._on_uav, 10) + self.create_subscription(Float64, '/winch/cmd_rate', self._on_rate, 10) + + self._length_pub = self.create_publisher(Float64, '/winch/length', 10) + self._free_pub = self.create_publisher(Float64, '/tether/free_length', 10) + self._taut_pub = self.create_publisher(Bool, '/tether/taut', 10) + self._stuck_pub = self.create_publisher(Bool, '/tether/stuck', 10) + self._obj_pub = self.create_publisher(PoseStamped, '/tether/object_pose', 10) + self._reached_pub = self.create_publisher(Bool, '/tether/goal_reached', 10) + self._marker_pub = self.create_publisher(MarkerArray, MARKER_TOPIC, 10) + + latched = QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + goal = self._pose(t.goal[0], t.goal[1], 0.0) + self.create_publisher(PoseStamped, '/tether/goal', latched).publish(goal) + + self.create_timer(self._dt, self._tick) + collisions = 'off' if self._obstacles is None else 'on' + self.get_logger().info( + f'tether: winch {t.winch[:2]} -> {t.ugv} -> {t.uav} -> object {t.object}, ' + f'goal {t.goal[:2]} r={t.goal[2]:g}, collisions {collisions}; ' + f'waiting for {t.ugv} and {t.uav} poses') + + def _on_ugv(self, msg): + p = msg.pose.position + self._ugv = (p.x, p.y, p.z) + + def _on_uav(self, msg): + p = msg.pose.position + self._uav = (p.x, p.y, p.z) + + def _on_rate(self, msg): + limit = self._t.max_reel_speed + self._rate = max(-limit, min(limit, msg.data)) + + def _pose(self, x, y, z): + pose = PoseStamped() + pose.header.stamp = self.get_clock().now().to_msg() + pose.header.frame_id = FRAME + pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = x, y, z + pose.pose.orientation.w = 1.0 + return pose + + def _tick(self): + if self._ugv is None or self._uav is None: + return + t = self._t + if self._state is None: + self._length = tether.initial_length(t.winch, self._ugv, self._uav, t.object, + self._attach_z, t.slack) + self.get_logger().info(f'tether ready: L = {self._length:.2f} m') + obj = t.object if self._state is None else self._state.obj + + was_stuck = self._state is not None and self._state.stuck + s = self._state = tether.step( + max(self._length + self._rate * self._dt, 0.0), t.winch, self._ugv, self._uav, + obj, self._attach_z, self._obstacles, t.object_radius) + self._length = s.length + if s.stuck and not was_stuck: + self.get_logger().info('object jammed against an obstacle') + + reached = tether.reached(s.obj, t.goal) + if reached and not self._reached: + self.get_logger().info('object reached the goal!') + self._reached = reached + + self._length_pub.publish(Float64(data=s.length)) + self._free_pub.publish(Float64(data=s.free)) + self._taut_pub.publish(Bool(data=s.taut)) + self._stuck_pub.publish(Bool(data=s.stuck)) + self._obj_pub.publish(self._pose(*s.obj, self._attach_z)) + self._reached_pub.publish(Bool(data=reached)) + + markers = self._dynamic_markers() + if self._ticks % self._static_every == 0: + markers += self._static_markers() + self._ticks += 1 + self._marker_pub.publish(MarkerArray(markers=markers)) + + def _marker(self, ns, marker_id, marker_type, x=0.0, y=0.0, z=0.0): + m = Marker() + m.header.frame_id = FRAME + m.ns = ns + m.id = marker_id + m.type = marker_type + m.action = Marker.ADD + m.pose.position.x, m.pose.position.y, m.pose.position.z = x, y, z + m.pose.orientation.w = 1.0 + return m + + def _static_markers(self): + t = self._t + wx, wy, wz = t.winch + base = self._marker('winch', 0, Marker.CUBE, wx, wy, wz / 2.0) + base.scale.x, base.scale.y, base.scale.z = 0.4, 0.4, wz + _color(base, 0.3, 0.33, 0.36) + drum = self._marker('winch', 1, Marker.CYLINDER, wx, wy, wz) + drum.scale.x, drum.scale.y, drum.scale.z = 0.3, 0.3, 0.12 + _color(drum, 0.91, 0.35, 0.05) + return [base, drum] + + def _dynamic_markers(self): + t = self._t + s = self._state + gx, gy, gr = t.goal + ox, oy = s.obj + size = t.object_size + + line = self._marker('tether', 0, Marker.LINE_STRIP) + line.scale.x = 0.03 + line.points = [Point(x=p[0], y=p[1], z=p[2]) + for p in (t.winch, self._ugv, self._uav, (ox, oy, self._attach_z))] + if s.taut: + _color(line, 0.91, 0.35, 0.05) + else: + _color(line, 0.98, 0.7, 0.45, 0.8) + + box = self._marker('object', 0, Marker.CUBE, ox, oy, size / 2.0) + box.scale.x = box.scale.y = box.scale.z = size + _color(box, 0.69, 0.54, 0.41) + + goal = self._marker('goal', 0, Marker.CYLINDER, gx, gy, 0.01) + goal.scale.x = goal.scale.y = 2.0 * gr + goal.scale.z = 0.02 + if self._reached: + _color(goal, 0.25, 0.75, 0.34, 0.9) + else: + _color(goal, 0.17, 0.54, 0.24, 0.35) + label = self._marker('goal', 1, Marker.TEXT_VIEW_FACING, gx, gy, 0.5) + label.scale.z = 0.25 + label.text = 'GOAL REACHED!' if self._reached else 'GOAL' + _color(label, 0.4, 0.9, 0.45) + + return [line, box, goal, label] + + +def main(args=None): + rclpy.init(args=args) + node = TetherSim() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/launch/octomap.launch.py b/launch/octomap.launch.py index cfef92a..29e57cf 100644 --- a/launch/octomap.launch.py +++ b/launch/octomap.launch.py @@ -1,31 +1,46 @@ -"""octomap_server for the scenario's sim.map (maps/.bt or a path; null disables it). +"""octomap_server for the scenario's map: its obstacles, or else its sim.map. ros2 launch hero_sim octomap.launch.py [config:=] + +A scenario with obstacles: gets an octomap of its pillars, written to +/hero_sim/_obstacles.bt; otherwise sim.map is served +(maps/.bt or a path; null: no octomap). Either way RViz shows it the same. """ +from pathlib import Path +import tempfile + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from hero_sim import config +from hero_sim import config, obstacles def _actions(context): config_path = config.resolve_config(LaunchConfiguration('config').perform(context)) - bt_file = config.resolve_map(config.load(config_path).sim['map']) - if bt_file is None: - return [LogInfo(msg='[hero_sim] sim.map is null: no octomap')] + scenario = config.load(config_path) + if scenario.obstacles: + bt_file = obstacles.write_pillar_map( + Path(tempfile.gettempdir()) / 'hero_sim' / f'{Path(config_path).stem}_obstacles.bt', + scenario.obstacles) + source = f'{len(scenario.obstacles)} pillar(s) from obstacles:, written to {bt_file}' + else: + bt_file = config.resolve_map(scenario.sim['map']) + if bt_file is None: + return [LogInfo(msg='[hero_sim] no obstacles: and sim.map is null: no octomap')] + source = bt_file return [ - LogInfo(msg=f'[hero_sim] octomap: {bt_file}'), + LogInfo(msg=f'[hero_sim] octomap: {source}'), Node(package='octomap_server', executable='octomap_server_node', name='octomap_server_node', output='screen', - parameters=[{'octomap_path': bt_file, 'frame_id': 'world'}]), + parameters=[{'octomap_path': str(bt_file), 'frame_id': 'world'}]), ] def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('config', default_value='', - description='Scenario YAML: a name in config/ or a path'), + description='Scenario YAML: a name in config/ or its folders, or a path'), OpaqueFunction(function=_actions), ]) diff --git a/launch/sim.launch.py b/launch/sim.launch.py index bf7276c..fb3d261 100644 --- a/launch/sim.launch.py +++ b/launch/sim.launch.py @@ -29,6 +29,9 @@ def _actions(context): Node(package='hero_sim', executable='ground_robot_sim', name='ground_robot_sim', output='screen', parameters=params), ] + if scenario.tether is not None: + actions.append(Node(package='hero_sim', executable='tether_sim', name='tether_sim', + output='screen', parameters=params)) if config.attach_enabled(sim): actions.append(Node(package='hero_sim', executable='sim_attach', name='sim_attach_node', output='screen', parameters=params)) @@ -48,6 +51,6 @@ def _actions(context): def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('config', default_value='', - description='Scenario YAML: a name in config/ or a path'), + description='Scenario YAML: a name in config/ or its folders, or a path'), OpaqueFunction(function=_actions), ]) diff --git a/media/tether_demo.png b/media/tether_demo.png new file mode 100644 index 0000000..db04751 Binary files /dev/null and b/media/tether_demo.png differ diff --git a/media/tether_demo.svg b/media/tether_demo.svg new file mode 100644 index 0000000..e3b7b71 --- /dev/null +++ b/media/tether_demo.svg @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + Tethered Object Transport (top view) + One winch, one UGV, one UAV, one tether. Slide the object across the floor, around the pillar, into the goal. + + + + + + + + x + y + + + + + GOAL + + + + + pillar + + + + + + + + straight pull jams + + + + 1 + + 2 + + + + + + + + + + then + fly here + + + + + + + + + + + + + W + Winch (fixed) + /winch/cmd_rate + + + + + + + + + + + G + UGV herover_1 + moving pulley on its deck + /herover_1/cmd_vel + + + + + + + + + + + A + UAV cf_1 + flying pulley + /cf_1/go_to + + + + + + O + Object + /tether/object_pose + + + d₁ = |W − G| + d₂ = |G − A| + l + + + + How the tether works + The object stays on the floor. + It can only be pulled, never lifted. + Tether paid out by the winch: L + l = L − |W−G| − |G−A| + free tether from the UAV to the object + • Slack: object stays put + • Taut: slides straight toward + the point under the UAV + • Pulled into the pillar: it jams. + Go around it in steps + + + + Your controls + 1. Drive the UGV + sets where the tether bends + 2. Fly the UAV + sets the pull direction + 3. Turn the winch + reel in (pull) / pay out (slack) + + Goal: object inside the + green circle + diff --git a/models/README.md b/models/README.md new file mode 100644 index 0000000..329fe30 --- /dev/null +++ b/models/README.md @@ -0,0 +1,12 @@ +# Robot meshes + +RViz draws Crazyflies, and ground robots with a `model:`, with the meshes in `models//`. They are simplified for RViz (herover 309k → 20k triangles, scout 349k → 12k). + +To add or re-make one, put the raw mesh in `models/source/` (not committed) and run: + +```bash +blender -b --factory-startup -P models/bake_mesh.py -- \ + "models/source/herover/hero mini rover.obj" models/herover/herover.glb 6000 -90 Y +``` + +Robots made of several `.dae` parts (like the scout, from agilex_sitl's meshes) are first combined into one OBJ with `models/assemble_dae.py`; its header has the command. Then register the mesh in `MODEL_MESHES` in `hero_sim/config.py`. diff --git a/setup.py b/setup.py index f0b4701..877170c 100644 --- a/setup.py +++ b/setup.py @@ -5,6 +5,14 @@ package_name = 'hero_sim' +# Scenario configs: config/*.yaml and its folders, layout kept (config.find_config). isfile +# skips broken links a --symlink-install build keeps to configs that moved or were deleted. +configs = [f for f in glob('config/**/*.yaml', recursive=True) if os.path.isfile(f)] +config_files = [ + (f'share/{package_name}/{d}', sorted(f for f in configs if os.path.dirname(f) == d)) + for d in sorted({os.path.dirname(f) for f in configs}) +] + # RViz meshes: models//*.{dae,glb,stl}. models/source/ (raw files) is not installed. model_files = [ (f'share/{package_name}/{d}', [f for ext in ('dae', 'glb', 'stl') for f in glob(f'{d}/*.{ext}')]) @@ -19,7 +27,8 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/config', glob('config/*.yaml') + glob('config/*.rviz')), + ('share/' + package_name + '/config', glob('config/*.rviz')), + *config_files, ('share/' + package_name + '/launch', glob('launch/*.launch.py')), ('share/' + package_name + '/maps', glob('maps/*.bt')), *model_files, @@ -33,6 +42,7 @@ 'ground_robot_sim = hero_sim.ground_robots:main', 'sim_attach = hero_sim.attach:main', 'robot_models = hero_sim.robot_models:main', + 'tether_sim = hero_sim.tether_sim:main', ], }, ) diff --git a/startup/_setup.sh b/startup/_setup.sh index 393fe53..414babd 100755 --- a/startup/_setup.sh +++ b/startup/_setup.sh @@ -2,37 +2,53 @@ # Source me: picks the scenario config and exports HERO_SIM_CONFIG. # # source _setup.sh # DEFAULT_CONFIG_NAME below -# source _setup.sh my.yaml # bare filename -> ../config/my.yaml -# source _setup.sh path/to/my.yaml # any path +# source _setup.sh my.yaml # bare filename, found in ../config/ or any folder in it +# source _setup.sh tether/my.yaml # relative to ../config/ ... +# source _setup.sh path/to/my.yaml # ... unless it's an existing path +# +# Filenames are unique across config/ and its folders (tether/, mrs/); a name +# in two folders is an error, so name the folder. Same rules as +# hero_sim/config.py find_config, which the launch files use. # # Order: the argument, then an already-exported HERO_SIM_CONFIG, then # DEFAULT_CONFIG_NAME. Edit DEFAULT_CONFIG_NAME to change your day-to-day default. -DEFAULT_CONFIG_NAME="test_branch.yaml" +DEFAULT_CONFIG_NAME="test_perching_full2.yaml" _hero_sim_config_dir=$(cd "$(dirname "${BASH_SOURCE[0]}")/../config" && pwd) _hero_sim_resolve() { case "$1" in - */*) echo "$1" ;; - *) echo "$_hero_sim_config_dir/$1" ;; + /*) echo "$1" ;; + */*) + if [ -f "$1" ]; then echo "$1"; else echo "$_hero_sim_config_dir/$1"; fi ;; + *) + local matches + matches=$(find "$_hero_sim_config_dir" -type f -name "$1" | sort) + if [ "$(printf '%s\n' "$matches" | grep -c .)" -gt 1 ]; then + echo "[hero_sim] ERROR: $1 is in more than one config folder; name the folder too:" >&2 + printf '%s\n' "$matches" | sed "s|^$_hero_sim_config_dir/| |" >&2 + return 1 + fi + echo "${matches:-$_hero_sim_config_dir/$1}" ;; esac } if [ -n "$1" ]; then - _hero_sim_cfg=$(_hero_sim_resolve "$1") + _hero_sim_name=$1 elif [ -n "$HERO_SIM_CONFIG" ] && [ -f "$HERO_SIM_CONFIG" ]; then - _hero_sim_cfg=$HERO_SIM_CONFIG + _hero_sim_name=$HERO_SIM_CONFIG else - _hero_sim_cfg=$(_hero_sim_resolve "$DEFAULT_CONFIG_NAME") + _hero_sim_name=$DEFAULT_CONFIG_NAME fi -if [ ! -f "$_hero_sim_cfg" ]; then - echo "[hero_sim] ERROR: config not found: $_hero_sim_cfg" >&2 +if ! _hero_sim_cfg=$(_hero_sim_resolve "$_hero_sim_name") || [ ! -f "$_hero_sim_cfg" ]; then + [ -n "$_hero_sim_cfg" ] && echo "[hero_sim] ERROR: config not found: $_hero_sim_cfg" >&2 unset -f _hero_sim_resolve + unset _hero_sim_cfg _hero_sim_name _hero_sim_config_dir return 1 2>/dev/null || exit 1 fi export HERO_SIM_CONFIG=$(realpath "$_hero_sim_cfg") echo "[hero_sim] HERO_SIM_CONFIG=$HERO_SIM_CONFIG" unset -f _hero_sim_resolve -unset _hero_sim_cfg _hero_sim_config_dir +unset _hero_sim_cfg _hero_sim_name _hero_sim_config_dir diff --git a/startup/start.sh b/startup/start.sh index ea67ac3..9c6d7c3 100755 --- a/startup/start.sh +++ b/startup/start.sh @@ -2,16 +2,18 @@ # Start the hero_sim tmux session (layout in session.yml). # # bash start.sh # DEFAULT_CONFIG_NAME from _setup.sh -# bash start.sh my.yaml # bare filename -> config/my.yaml -# bash start.sh path/to/my.yaml # any path (relative to where you run this) +# bash start.sh my.yaml # bare filename, found in config/ or any folder in it +# bash start.sh tether/my.yaml # relative to config/ ... +# bash start.sh path/to/my.yaml # ... unless it's an existing path (relative to where you run this) # # Stop it with: tmux kill-session -t hero_sim set -e CONFIG_ARG=$1 -# Resolve a relative path before changing directory. +# Resolve an existing relative path before changing directory; anything else +# (tether/my.yaml) is looked up under config/ by _setup.sh. case "$CONFIG_ARG" in - */*) CONFIG_ARG=$(realpath "$CONFIG_ARG") ;; + */*) [ -f "$CONFIG_ARG" ] && CONFIG_ARG=$(realpath "$CONFIG_ARG") ;; esac cd "$(dirname "$(readlink -f "$0")")" diff --git a/test/conftest.py b/test/conftest.py index 9ad6fdc..4e8ed9b 100644 --- a/test/conftest.py +++ b/test/conftest.py @@ -36,3 +36,12 @@ def write(text): def duration(sec, nanosec=0): from builtin_interfaces.msg import Duration return Duration(sec=sec, nanosec=nanosec) + + +def wall_map(max_radius=0.5): + """ObstacleMap with a 1 m long wall along y, face at x = 1.0, 0.1 m thick, on a 0.1 m grid.""" + import numpy as np + from hero_sim.obstacles import ObstacleMap + occupied = np.zeros((10, 2), bool) + occupied[:, 0] = True + return ObstacleMap(occupied, 1.0, -0.5, 0.1, max_radius) diff --git a/test/test_config.py b/test/test_config.py index 9604a1f..04fd85c 100644 --- a/test/test_config.py +++ b/test/test_config.py @@ -1,3 +1,5 @@ +from pathlib import Path + import pytest import yaml @@ -45,11 +47,53 @@ def test_model_meshes_exist(): assert (CONFIG_DIR.parent / 'models' / rel).is_file(), rel +def test_footprint_radius_defaults_by_model_and_overrides(): + s = parse(''' +ground_robots: + - {name: s, model: scout} + - {name: h, model: herover} + - {name: r} + - {name: w, model: scout, radius: 0.5} +crazyflies: [{pose: {}}, {pose: {}, radius: 0.2}] +''') + assert [r.radius for r in s.ground_robots] == [0.42, 0.17, config.DEFAULT_GROUND_RADIUS, 0.5] + assert [cf.radius for cf in s.crazyflies] == [config.FOOTPRINT_RADII['crazyflie'], 0.2] + + +def test_ground_pose_z_defaults_to_marker_height_and_overrides(): + s = parse(''' +ground_robots: + - {name: s, model: scout} + - {name: h, model: herover, pose: {x: 1.0}} + - {name: r} + - {name: w, model: scout, pose: {z: 0.3}} +''') + assert [r.z for r in s.ground_robots] == [config.MARKER_HEIGHTS['scout'], + config.MARKER_HEIGHTS['herover'], 0.0, 0.3] + + +def test_collision_z_becomes_a_float_pair(): + assert parse('sim: {collision_z: [0, 3]}').sim['collision_z'] == (0.0, 3.0) + + def test_crazyflie_gripper_is_optional(): s = parse('crazyflies: [{pose: {}, gripper: true}, {pose: {}}]') assert [cf.gripper for cf in s.crazyflies] == [True, False] +def test_obstacles_are_optional_and_parsed(): + assert parse('{}').obstacles == () + s = parse('obstacles: [{x: 1, y: 2, radius: 0.5}, {x: -1, y: 0, radius: 0.1, height: 0.4}]') + assert s.obstacles == ((1.0, 2.0, 0.5, config.DEFAULT_PILLAR_HEIGHT), (-1.0, 0.0, 0.1, 0.4)) + assert s.obstacles[1].height == 0.4 + + +def test_obstacles_replace_the_default_map(): + assert parse('{}').sim['map'] == config.SIM_DEFAULTS['map'] + assert parse('obstacles: [{x: 1, y: 2, radius: 0.5}]').sim['map'] is None + assert parse('obstacles: [{x: 1, y: 2, radius: 0.5}]\nsim: {map: null}').sim['map'] is None + + def test_drive_is_case_insensitive(): s = parse('ground_robots: [{name: h, drive: DIFF}]') assert s.ground_robots[0].drive == 'diff' @@ -57,7 +101,7 @@ def test_drive_is_case_insensitive(): @pytest.mark.parametrize('text, match', [ ('robots: []', "unknown key.*'robots'"), - ('ground_robots: [{name: a, pose: {x: 1, z: 2}}]', r"ground_robots\[0\] \(a\)\.pose: unknown key.*'z'"), + ('ground_robots: [{name: a, pose: {x: 1, roll: 2}}]', r"ground_robots\[0\] \(a\)\.pose: unknown key.*'roll'"), ('ground_robots: [{name: a, speed: 1}]', "unknown key.*'speed'"), ('ground_robots: [{pose: {x: 1}}]', 'name is required'), ('ground_robots: [{name: a, drive: tank}]', 'drive must be one of'), @@ -71,6 +115,20 @@ def test_drive_is_case_insensitive(): ('sim: {ground_rate: 0}', 'ground_rate must be > 0'), ('sim: {cf_battery_drain: -1}', 'cf_battery_drain must be >= 0'), ('sim: {attach: maybe}', 'attach must be auto, true or false'), + ('sim: {collision: maybe}', 'collision must be true or false'), + ('sim: {collision_z: 1.0}', r'collision_z must be \[z_min, z_max\]'), + ('sim: {collision_z: [2.0, 1.0]}', r'collision_z must be \[z_min, z_max\]'), + ('ground_robots: [{name: a, radius: -1}]', r'\(a\): radius must be >= 0'), + ('crazyflies: [{radius: -0.1}]', r'\(cf_1\): radius must be >= 0'), + ('obstacles: {x: 0}', 'obstacles must be a list'), + ('obstacles: [{x: 0, y: 0}]', r"obstacles\[0\]: missing \['radius'\]"), + ('obstacles: [[0, 0, 1]]', r'obstacles\[0\] must be a mapping'), + ('obstacles: [{x: 0, y: 0, radius: 1, z: 2}]', r"obstacles\[0\]: unknown key.*'z'"), + ('obstacles: [{x: 0, y: 0, radius: 1}, {x: 0, y: 0, radius: 0}]', + r'obstacles\[1\].radius must be > 0'), + ('obstacles: [{x: 0, y: 0, radius: 1, height: 0}]', r'obstacles\[0\].height must be > 0'), + ('obstacles: [{x: 0, y: 0, radius: 1}]\nsim: {map: icuas26_1}', + 'sim.map and obstacles: are both set'), ]) def test_rejects_bad_config(text, match): with pytest.raises(ValueError, match=match): @@ -121,6 +179,113 @@ def test_resolve_map_disabled_and_path(tmp_path): assert config.resolve_map(str(bt)) == str(bt) -@pytest.mark.parametrize('path', sorted(CONFIG_DIR.glob('*.yaml')), ids=lambda p: p.name) +SHIPPED = sorted(CONFIG_DIR.rglob('*.yaml')) + + +@pytest.mark.parametrize('path', SHIPPED, ids=lambda p: str(p.relative_to(CONFIG_DIR))) def test_shipped_configs_load(path): config.load(path) + + +def test_shipped_config_names_are_unique_across_folders(): + names = [p.name for p in SHIPPED] + assert sorted({n for n in names if names.count(n) > 1}) == [] + assert any(p.parent != CONFIG_DIR for p in SHIPPED) # the folders are searched at all + + +@pytest.fixture() +def config_tree(tmp_path): + """config/ with a.yaml on top, tether/b.yaml, and dup.yaml in two folders.""" + root = tmp_path / 'config' + for rel in ('a.yaml', 'tether/b.yaml', 'mrs/dup.yaml', 'tether/dup.yaml'): + (root / rel).parent.mkdir(parents=True, exist_ok=True) + (root / rel).write_text('{}\n') + return root + + +def test_find_config_by_name_in_any_folder(config_tree): + assert config.find_config('a.yaml', config_tree) == config_tree / 'a.yaml' + assert config.find_config('b.yaml', config_tree) == config_tree / 'tether' / 'b.yaml' + assert config.find_config('tether/b.yaml', config_tree) == config_tree / 'tether' / 'b.yaml' + assert config.find_config('mrs/dup.yaml', config_tree) == config_tree / 'mrs' / 'dup.yaml' + + +def test_find_config_prefers_an_existing_path(config_tree, tmp_path, monkeypatch): + (tmp_path / 'tether').mkdir() + (tmp_path / 'tether' / 'b.yaml').write_text('{}\n') + monkeypatch.chdir(tmp_path) + assert config.find_config('tether/b.yaml', config_tree) == Path('tether/b.yaml') + assert config.find_config(str(tmp_path / 'config' / 'a.yaml'), config_tree).is_file() + + +def test_find_config_errors(config_tree): + with pytest.raises(ValueError, match=r'dup.yaml is in more than one folder: mrs/dup.yaml, ' + r'tether/dup.yaml'): + config.find_config('dup.yaml', config_tree) + with pytest.raises(FileNotFoundError, match='nope.yaml'): + config.find_config('nope.yaml', config_tree) + with pytest.raises(FileNotFoundError, match='mrs/b.yaml'): + config.find_config('mrs/b.yaml', config_tree) # wrong folder + + +TETHER_ROBOTS = 'ground_robots: [{name: g1}, {name: g2}]\ncrazyflies: [{pose: {}}, {pose: {}}]\n' +TETHER_MIN = '{winch: {x: -1, y: 0}, object: {x: 1, y: 2}, goal: {x: 3, y: 4}' + + +def test_tether_is_optional(): + assert parse(TETHER_ROBOTS).tether is None + + +def test_tether_defaults(): + t = parse(TETHER_ROBOTS + f'tether: {TETHER_MIN}}}').tether + assert (t.ugv, t.uav) == ('g1', 'cf_1') + assert t.winch == (-1.0, 0.0, 0.3) + assert t.object == (1.0, 2.0) + assert t.goal == (3.0, 4.0, 0.4) + assert (t.object_size, t.object_radius, t.slack, t.max_reel_speed) == (0.3, 0.15, 0.5, 0.5) + + +def test_tether_overrides(): + t = parse(TETHER_ROBOTS + f'tether: {TETHER_MIN}, ugv: g2, uav: cf_2, ' + 'object_size: 0.2}').tether + assert (t.ugv, t.uav, t.object_radius) == ('g2', 'cf_2', 0.1) + + +@pytest.mark.parametrize('extra, match', [ + (', rope: 1', "tether: unknown key.*'rope'"), + (', ugv: nope', 'tether.ugv must name a ground robot'), + (', uav: cf_9', r'tether.uav must name a crazyflie'), + (', goal: {x: 3, y: 4, z: 0}', r"tether.goal: unknown key.*'z'"), + (', goal: {x: 3, y: 4, radius: 0}', 'tether.goal.radius must be > 0'), + (', object: {x: 1}', r"tether.object: missing \['y'\]"), + (', obstacles: [{x: 0, y: 0, radius: 1}]', 'tether.obstacles moved to the top-level'), + (', object_size: 0', 'tether.object_size must be > 0'), + (', max_reel_speed: -1', 'tether.max_reel_speed must be > 0'), + (', slack: -1', 'tether.slack must be >= 0'), +]) +def test_rejects_bad_tether(extra, match): + with pytest.raises(ValueError, match=match): + parse(TETHER_ROBOTS + f'tether: {TETHER_MIN}{extra}}}') + + +def test_tether_object_must_start_clear_of_pillars(): + clear = TETHER_ROBOTS + 'obstacles: [{x: 1.4, y: 2, radius: 0.2}]\n' + assert parse(clear + f'tether: {TETHER_MIN}}}').obstacles == ((1.4, 2.0, 0.2, 2.0),) + inside = TETHER_ROBOTS + 'obstacles: [{x: 0, y: 0, radius: 0.1}, {x: 1.3, y: 2, radius: 0.2}]' + with pytest.raises(ValueError, match=r'tether.object starts inside obstacles\[1\]'): + parse(inside + f'\ntether: {TETHER_MIN}}}') + + +@pytest.mark.parametrize('text, match', [ + ('tether: [1]', 'tether must be a mapping'), + ('tether: {winch: {x: 0, y: 0}, object: {x: 1, y: 0}, goal: {x: 2, y: 0}}', + 'tether.ugv must name a ground robot'), + ('ground_robots: [{name: g1}]\n' + 'tether: {winch: {x: 0, y: 0}, object: {x: 1, y: 0}, goal: {x: 2, y: 0}}', + 'tether.uav must name a crazyflie'), + (TETHER_ROBOTS + 'tether: {object: {x: 1, y: 0}, goal: {x: 2, y: 0}}', + r"tether.winch: missing \['x', 'y'\]"), +]) +def test_rejects_tether_without_its_parts(text, match): + with pytest.raises(ValueError, match=match): + parse(text) diff --git a/test/test_crazyflie.py b/test/test_crazyflie.py index d73d9d5..00e29ef 100644 --- a/test/test_crazyflie.py +++ b/test/test_crazyflie.py @@ -5,7 +5,7 @@ pytest.importorskip('rclpy') pytest.importorskip('crazyflie_interfaces') -from conftest import duration # noqa: E402 +from conftest import duration, wall_map # noqa: E402 from crazyflie_interfaces.msg import FullState, Hover, Position, Status, VelocityWorld # noqa: E402 from crazyflie_interfaces.srv import Arm, GoTo, Land, NotifySetpointsStop, Takeoff # noqa: E402 from geometry_msgs.msg import Point, Quaternion # noqa: E402 @@ -194,3 +194,33 @@ def test_update_returns_world_transform(cf): assert (t.header.frame_id, t.child_frame_id) == ('world', 'cf_test') assert (t.transform.translation.x, t.transform.translation.y, t.transform.translation.z) == (1.0, 2.0, 3.0) + + +@pytest.fixture() +def walled_cf(node): + """Drone at the origin with a wall 1 m ahead in +x (conftest.wall_map).""" + return SimCrazyflie(node, 'cf_test', (0.0, 0.0, 0.0), dt=0.1, radius=0.1, + obstacles=wall_map()) + + +def test_obstacle_stops_go_to_but_not_height(walled_cf): + takeoff(walled_cf, height=1.0, sec=1) + run(walled_cf, 12) + go_to(walled_cf, 2, 0, 1.5, sec=2) + run(walled_cf, 25) + assert 0.7 < walled_cf.position[0] < 1.0 - walled_cf.radius + assert walled_cf.position[2] == pytest.approx(1.5) + + +def test_obstacle_stops_setpoints(walled_cf): + walled_cf._cmd_position_cb(Position(x=1.5, y=0.0, z=1.0, yaw=0.0)) + assert walled_cf.position[0] < 0.9 and walled_cf.position[2] == 1.0 + walled_cf._cmd_position_cb(Position(x=0.0, y=2.0, z=1.0, yaw=0.0)) # clear of the wall + np.testing.assert_allclose(walled_cf.position, [0, 2, 1]) + + +def test_set_position_teleports_past_obstacles(walled_cf): + req = GoTo.Request(goal=Point(x=1.5, y=0.0, z=1.0)) + walled_cf._set_position_cb(req, GoTo.Response()) + walled_cf.update() + assert walled_cf.position[0] == 1.5 diff --git a/test/test_kinematics.py b/test/test_kinematics.py index 753586a..7d9fda6 100644 --- a/test/test_kinematics.py +++ b/test/test_kinematics.py @@ -3,7 +3,7 @@ import pytest from hero_sim.kinematics import ( - integrate_ground, limit_ground_cmd, quat_to_yaw, wrap_angle, yaw_to_quat, + GroundGoTo, integrate_ground, limit_ground_cmd, quat_to_yaw, wrap_angle, yaw_to_quat, ) @@ -52,3 +52,53 @@ def test_integrate_full_turn_returns_to_start(): for _ in range(steps): x, y, yaw = integrate_ground(x, y, yaw, 1.0, 0.0, 1.0, dt) assert (x, y) == pytest.approx((0.0, 0.0), abs=0.02) + + +def test_go_to_omni_goes_straight_in_the_duration(): + g = GroundGoTo((0.0, 0.0, 0.0), (1.0, 1.0, 1.0), 'omni', 4.0, 2.0, 2.0) + assert g.total == 4.0 + assert g.at(2.0) == pytest.approx((0.5, 0.5, 0.5)) + assert g.at(4.0) == pytest.approx((1.0, 1.0, 1.0)) + assert g.at(9.0) == pytest.approx((1.0, 1.0, 1.0)) + + +def test_go_to_held_to_the_speed_limits(): + assert GroundGoTo((0, 0, 0), (4, 0, 0), 'omni', 1.0, 2.0, 2.0).total == pytest.approx(2.0) + assert GroundGoTo((0, 0, 0), (0, 0, 3), 'omni', 0.0, 2.0, 1.0).total == pytest.approx(3.0) + + +def test_go_to_turns_the_short_way(): + g = GroundGoTo((0.0, 0.0, 3.0), (0.0, 0.0, -3.0), 'omni', 0.0, 2.0, 1.0) + assert g.total == pytest.approx(2 * math.pi - 6.0) # through pi, not 6 rad back + assert abs(g.at(g.total / 2)[2]) == pytest.approx(math.pi) + + +def test_go_to_diff_turns_drives_then_turns(): + # Facing +x, goal 2 m up +y facing -x: 90 deg in place, 2 m straight, 90 deg in place. + g = GroundGoTo((0.0, 0.0, 0.0), (0.0, 2.0, math.pi), 'diff', 0.0, + max_speed=1.0, max_yaw_rate=math.pi / 2) + assert g.total == pytest.approx(4.0) + assert g.at(0.5) == pytest.approx((0.0, 0.0, math.pi / 4)) + assert g.at(2.0) == pytest.approx((0.0, 1.0, math.pi / 2)) + assert g.at(3.5) == pytest.approx((0.0, 2.0, 3 * math.pi / 4)) + assert g.at(4.0) == pytest.approx((0.0, 2.0, math.pi)) + + +def test_go_to_diff_stretches_every_phase_alike(): + g = GroundGoTo((0.0, 0.0, 0.0), (0.0, 2.0, math.pi), 'diff', 8.0, 1.0, math.pi / 2) + assert g.total == 8.0 + assert g.at(1.0) == pytest.approx((0.0, 0.0, math.pi / 4)) # half the yaw rate + assert g.at(4.0) == pytest.approx((0.0, 1.0, math.pi / 2)) # half the speed + + +def test_go_to_diff_barely_moving_only_turns(): + g = GroundGoTo((1.0, 1.0, 0.0), (1.005, 1.0, 1.0), 'diff', 0.0, 1.0, 2.0) + assert g.total == pytest.approx(0.5) + assert g.at(0.25)[2] == pytest.approx(0.5) + assert g.at(0.5) == pytest.approx((1.005, 1.0, 1.0)) + + +def test_go_to_nowhere_is_done_at_once(): + g = GroundGoTo((1.0, 2.0, 0.5), (1.0, 2.0, 0.5), 'omni', 0.0, 2.0, 2.0) + assert g.total == 0.0 + assert g.at(0.0) == pytest.approx((1.0, 2.0, 0.5)) diff --git a/test/test_nodes.py b/test/test_nodes.py index 22c90e2..f9b59c6 100644 --- a/test/test_nodes.py +++ b/test/test_nodes.py @@ -7,10 +7,11 @@ pytest.importorskip('crazyflie_interfaces') from conftest import duration # noqa: E402 -from crazyflie_interfaces.srv import Arm, Takeoff # noqa: E402 +from crazyflie_interfaces.srv import Arm, GoTo, Takeoff # noqa: E402 from std_srvs.srv import Trigger # noqa: E402 -from geometry_msgs.msg import PoseStamped, Twist # noqa: E402 +from geometry_msgs.msg import Point, PoseStamped, Twist # noqa: E402 +from hero_sim import config # noqa: E402 from hero_sim.kinematics import yaw_to_quat # noqa: E402 SCENARIO = """ @@ -19,7 +20,7 @@ pose: {x: 1.0, y: 0.0, yaw: 1.5707963267948966} model: scout - name: herover_1 - pose: {x: 0.0, y: 0.0, yaw: 0.0} + pose: {x: 0.0, y: -1.0, yaw: 0.0} # clear of scout_1's path in the cmd_vel test drive: diff crazyflies: - pose: {x: 0.5, y: 0.5, z: 0.03} @@ -114,18 +115,258 @@ def test_ground_robots_publish_pose(make): assert got, 'no pose received on /scout_1/pose' assert got[-1].header.frame_id == 'world' assert got[-1].pose.position.x == pytest.approx(1.0) + assert got[-1].pose.position.z == pytest.approx(config.MARKER_HEIGHTS['scout']) -def test_robot_models_latched_and_frame_locked(make): - from rclpy.qos import DurabilityPolicy, QoSProfile - from visualization_msgs.msg import Marker, MarkerArray - from hero_sim.robot_models import TOPIC, RobotModels +def test_ground_robots_stop_at_map_obstacles(make, scenario_file): + from hero_sim.ground_robots import GroundRobotSim + # Heading +x into the icuas26_1 trunk whose west face is at x = -3.3. + scenario_file('ground_robots: [{name: r, pose: {x: -4.5, y: 0.15}, radius: 0.3}]\n' + 'sim: {ground_rate: 10.0}\n') + sim = make(GroundRobotSim) + robot = sim._robots[0] + sim._on_cmd_vel(robot, twist(vx=1.0)) + for _ in range(30): # 3 s at 1 m/s would carry it through the trunk + sim._tick() + assert robot.x == pytest.approx(-3.3 - robot.radius, abs=0.15) + assert sim._obstacles.clearance(robot.x, robot.y) >= robot.radius + + sim._on_cmd_vel(robot, twist(vx=-1.0)) # backing away is never blocked + sim._tick() + assert robot.x == pytest.approx(-3.3 - robot.radius - 0.1, abs=0.15) + + +PILLAR_SCENARIO = """ +ground_robots: + - name: r + pose: {x: 0.0, y: 0.0} + radius: 0.3 +crazyflies: + - pose: {x: 0.0, y: 1.0, z: 1.0} +obstacles: + - {x: 1.0, y: 0.0, radius: 0.2} + - {x: 1.0, y: 1.0, radius: 0.2, height: 0.5} +sim: {map: null, ground_rate: 10.0, cf_rate: 10.0} +""" + + +def test_ground_robots_stop_at_pillars(make, scenario_file): + from hero_sim.ground_robots import GroundRobotSim + scenario_file(PILLAR_SCENARIO) + sim = make(GroundRobotSim) + robot = sim._robots[0] + sim._on_cmd_vel(robot, twist(vx=1.0)) + for _ in range(20): # 2 s at 1 m/s would carry it through the pillar + sim._tick() + assert 0.5 - 0.05 <= robot.x <= 0.5 # pillar edge at 0.8, minus the radius + + sim._on_cmd_vel(robot, twist(vy=1.0)) # sideways past it is free + sim._tick() + assert robot.y == pytest.approx(0.1) + + +def test_crazyflies_stop_at_pillars_at_any_height(make, scenario_file): + from hero_sim.crazyflie_server import CrazyflieServer + scenario_file(PILLAR_SCENARIO) + server = make(CrazyflieServer) + cf = server.drones['cf_1'] + req = GoTo.Request(goal=Point(x=2.0, y=1.0, z=1.5), duration=duration(1)) + cf._go_to_cb(req, GoTo.Response()) + for _ in range(20): + server._update_all() + # Pillar edge at 0.8, minus 0.1: the pillar is 0.5 m tall, but blocks at any height. + assert 0.7 - 0.05 <= cf.position[0] <= 0.7 + assert cf.position[2] == pytest.approx(1.5) # height still goes through + + +def test_collision_off_ignores_pillars(make, scenario_file): + from hero_sim.ground_robots import GroundRobotSim + scenario_file(PILLAR_SCENARIO.replace('map: null', 'map: null, collision: false')) + sim = make(GroundRobotSim) + assert sim._obstacles is None + robot = sim._robots[0] + sim._on_cmd_vel(robot, twist(vx=1.0)) + for _ in range(20): + sim._tick() + assert robot.x == pytest.approx(2.0) + + +GO_TO_SCENARIO = """ +ground_robots: + - name: omni_1 + pose: {x: 0.0, y: 0.0} + - name: diff_1 + pose: {x: 0.0, y: 2.0} + drive: diff +obstacles: + - {x: 3.0, y: 0.0, radius: 0.2} +sim: {map: null, ground_rate: 10.0, ground_max_speed: 1.0, ground_max_yaw_rate: 2.0} +""" + + +def go_to(x, y, yaw=0.0, sec=0, relative=False): + return GoTo.Request(goal=Point(x=x, y=y, z=5.0), yaw=yaw, duration=duration(sec), + relative=relative) + + +@pytest.fixture() +def ground(make, scenario_file): + from hero_sim.ground_robots import GroundRobotSim + scenario_file(GO_TO_SCENARIO) + return make(GroundRobotSim) + + +def test_ground_go_to_reaches_the_goal(ground): + omni, _ = ground._robots + ground._on_go_to(omni, go_to(1.0, 0.5, yaw=0.5, sec=2), GoTo.Response()) + assert omni.go_to.total == 2.0 + for _ in range(10): + ground._tick() + assert (omni.x, omni.y, omni.yaw) == pytest.approx((0.5, 0.25, 0.25)) + for _ in range(15): + ground._tick() + assert (omni.x, omni.y, omni.yaw) == pytest.approx((1.0, 0.5, 0.5)) + assert omni.go_to is None # done: the robot holds still + ground._tick() + assert (omni.x, omni.y) == pytest.approx((1.0, 0.5)) + + +def test_ground_go_to_held_to_max_speed(ground): + omni, _ = ground._robots + ground._on_go_to(omni, go_to(2.0, 0.0), GoTo.Response()) # duration 0 + assert omni.go_to.total == pytest.approx(2.0) # ground_max_speed 1 m/s + for _ in range(10): + ground._tick() + assert omni.x == pytest.approx(1.0) + + +def test_ground_go_to_relative_and_diff(ground): + _, diff = ground._robots + ground._on_go_to(diff, go_to(1.0, 0.0, relative=True), GoTo.Response()) + for _ in range(15): + ground._tick() + assert (diff.x, diff.y, diff.yaw) == pytest.approx((1.0, 2.0, 0.0)) + + +def test_cmd_vel_cancels_a_go_to(ground): + omni, _ = ground._robots + ground._on_go_to(omni, go_to(2.0, 0.0, sec=4), GoTo.Response()) + for _ in range(5): + ground._tick() + ground._on_cmd_vel(omni, twist()) + assert omni.go_to is None + stopped = omni.x + for _ in range(5): + ground._tick() + assert omni.x == stopped == pytest.approx(0.25) + + +def test_ground_go_to_stops_at_a_pillar(ground): + omni, _ = ground._robots + ground._on_go_to(omni, go_to(5.0, 0.0), GoTo.Response()) + for _ in range(60): + ground._tick() + assert 2.5 - 0.05 <= omni.x <= 2.5 # pillar edge at 2.8, minus the radius 0.3 + assert omni.go_to is not None # past its 5 s, still waiting for the way to clear + ground._on_cmd_vel(omni, twist()) + assert omni.go_to is None + + +def test_ground_go_to_service_call(ground, make): + client = make(lambda: rclpy.create_node('go_to_client')) + cli = client.create_client(GoTo, '/diff_1/go_to') + assert cli.wait_for_service(timeout_sec=2.0) + executor = rclpy.executors.SingleThreadedExecutor() + executor.add_node(ground) + executor.add_node(client) + future = cli.call_async(go_to(1.0, 3.0, sec=2)) + executor.spin_until_future_complete(future, timeout_sec=2.0) + executor.shutdown() + assert future.done() + goal = ground._robots[1].go_to.goal + assert goal == pytest.approx((1.0, 3.0, 0.0)) + + +TWO_SCENARIO = """ +ground_robots: + - name: a + pose: {x: 0.0, y: 0.0} + radius: 0.3 + - name: b + pose: {x: 2.0, y: 0.0, yaw: 3.141592653589793} + radius: 0.2 +sim: {map: null, ground_rate: 10.0, ground_max_speed: 1.0} +""" + - publisher = make(RobotModels) # publishes once, before anyone subscribes +def gap(a, b): + """Distance between the edges of two robots' footprints; < 0 when they overlap.""" + return math.hypot(a.x - b.x, a.y - b.y) - a.radius - b.radius + + +def test_ground_robots_stop_at_each_other(make, scenario_file): + from hero_sim.ground_robots import GroundRobotSim + scenario_file(TWO_SCENARIO) + sim = make(GroundRobotSim) + a, b = sim._robots + sim._on_cmd_vel(a, twist(vx=1.0)) + sim._on_cmd_vel(b, twist(vx=1.0)) # b faces -x: head-on + for _ in range(20): + sim._tick() + assert gap(a, b) >= 0.0 + assert gap(a, b) <= 0.05 # touching, within a path sample each + assert (a.y, b.y) == pytest.approx((0.0, 0.0)) + + sim._on_cmd_vel(a, twist(vx=-1.0)) # backing away is free, b still pushing + sim._on_cmd_vel(b, twist()) + sim._tick() + assert gap(a, b) == pytest.approx(0.1, abs=0.05) + + +@pytest.mark.parametrize('change', ['radius: 0.2|radius: 0.0', + 'map: null|map: null, collision: false']) +def test_ground_robots_pass_through_without_collisions(make, scenario_file, change): + from hero_sim.ground_robots import GroundRobotSim + scenario_file(TWO_SCENARIO.replace(*change.split('|'))) + sim = make(GroundRobotSim) + a, _ = sim._robots + sim._on_cmd_vel(a, twist(vx=1.0)) + for _ in range(30): + sim._tick() + assert a.x == pytest.approx(3.0) + + +def test_blocked_go_to_waits_then_carries_on(make, scenario_file): + from hero_sim.ground_robots import GroundRobotSim + scenario_file(TWO_SCENARIO) + sim = make(GroundRobotSim) + a, b = sim._robots + sim._on_go_to(a, go_to(4.0, 0.0), GoTo.Response()) # 4 s at 1 m/s, b in the way + for _ in range(50): + sim._tick() + assert 1.5 - 0.05 <= a.x <= 1.5 and a.go_to is not None # waiting at b + + sim._on_cmd_vel(b, twist(vy=1.0)) # b faces -x: its left is -y + for i in range(50): + if i == 10: + sim._on_cmd_vel(b, twist()) # b parked at (2, -1) + before = a.x + sim._tick() + assert gap(a, b) >= 0.0 + assert a.x - before <= 0.1 + 0.03 # resumes at its speed, no jump to the schedule + assert b.y == pytest.approx(-1.0) + assert (a.x, a.y) == pytest.approx((4.0, 0.0)) and a.go_to is None + + +def _latched(make, topic, publisher_cls): + """The first MarkerArray a late TRANSIENT_LOCAL subscriber gets on `topic`.""" + from rclpy.qos import DurabilityPolicy, QoSProfile + from visualization_msgs.msg import MarkerArray + publisher = make(publisher_cls) # publishes once, before anyone subscribes got = [] listener = make(lambda: rclpy.create_node('rviz_stand_in')) listener.create_subscription( - MarkerArray, TOPIC, got.append, + MarkerArray, topic, got.append, QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(publisher) @@ -135,15 +376,39 @@ def test_robot_models_latched_and_frame_locked(make): if got: break executor.shutdown() - assert got, 'late subscriber did not get the latched MarkerArray' + assert got, f'late subscriber did not get the latched MarkerArray on {topic}' + return got[0] - markers = got[0].markers + +def test_pillars_drawn_latched_even_with_collision_off(make, scenario_file): + from visualization_msgs.msg import Marker + from hero_sim.ground_robots import OBSTACLE_TOPIC, GroundRobotSim + + scenario_file(PILLAR_SCENARIO.replace('map: null', 'map: null, collision: false')) + markers = _latched(make, OBSTACLE_TOPIC, GroundRobotSim).markers + assert markers[0].action == Marker.DELETEALL + pillars = markers[1:] + assert [(m.ns, m.id, m.type, m.header.frame_id) for m in pillars] == [ + ('obstacles', 0, Marker.CYLINDER, 'world'), ('obstacles', 1, Marker.CYLINDER, 'world')] + assert [(m.pose.position.x, m.pose.position.y) for m in pillars] == [(1.0, 0.0), (1.0, 1.0)] + assert all(m.scale.x == m.scale.y == pytest.approx(0.4) for m in pillars) + assert [(m.scale.z, m.pose.position.z) for m in pillars] == [(2.0, 1.0), (0.5, 0.25)] + + +def test_robot_models_latched_and_frame_locked(make): + from visualization_msgs.msg import Marker + from hero_sim.robot_models import TOPIC, RobotModels + + markers = _latched(make, TOPIC, RobotModels).markers # herover_1 has no model -> TF only; scout_1 and both CFs get meshes assert [(m.header.frame_id, m.ns) for m in markers] == [ ('cf_1', 'crazyflie'), ('cf_2', 'crazyflie'), ('scout_1', 'scout')] assert all(m.type == Marker.MESH_RESOURCE and m.frame_locked and m.mesh_use_embedded_materials for m in markers) assert markers[2].mesh_resource == 'package://hero_sim/models/scout/scout.glb' + # CF meshes sit on their frame; the scout's drops from its marker height to the floor + assert [m.pose.position.z for m in markers] == pytest.approx( + [0.0, 0.0, -config.MARKER_HEIGHTS['scout']]) assert len({(m.ns, m.id) for m in markers}) == len(markers) @@ -187,7 +452,6 @@ def test_gripper_claws_shut_around_the_bar(): def test_grippers_lifecycle(rclpy_context, node): from visualization_msgs.msg import Marker - from hero_sim import config from hero_sim.gripper import CLOSED, OPEN, Grippers cfs = (config.Crazyflie('cf_1', 0, 0, 0, gripper=True), config.Crazyflie('cf_2', 0, 0, 0)) @@ -218,7 +482,6 @@ def test_grippers_lifecycle(rclpy_context, node): def test_grippers_not_drawn_without_models(rclpy_context, node): - from hero_sim import config from hero_sim.gripper import TOPIC, Grippers grippers = Grippers(node, (config.Crazyflie('cf_1', 0, 0, 0, gripper=True),), draw=False) @@ -294,3 +557,82 @@ def test_attach_fails_without_pose(make): req = AttachAgent.Request(child_id='cf_9', parent_id='ghost', attach=True) resp = node._on_request(req, AttachAgent.Response()) assert not resp.success and 'ghost' in resp.message + + +TETHER_SCENARIO = """ +ground_robots: + - name: ugv + pose: {x: -1.0, y: 0.0} +crazyflies: + - pose: {x: 1.0, y: 0.0, z: 1.0} +tether: + winch: {x: -2.0, y: 0.0, z: 0.0} + object: {x: 3.0, y: 0.0} + object_size: 0.2 + goal: {x: 1.0, y: 0.0, radius: 0.3} + slack: 0.0 + max_reel_speed: 1.0 +sim: {ground_rate: 10.0, map: null} +""" + + +def pose_msg(x, y, z): + msg = PoseStamped() + msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = x, y, z + return msg + + +def test_tether_sim_reels_the_object_in(rclpy_context, scenario_file): + from std_msgs.msg import Float64 + from hero_sim.tether_sim import TetherSim + + scenario_file(TETHER_SCENARIO) + node = TetherSim() + try: + node._tick() # no poses yet: waits + assert node._state is None + + node._on_ugv(pose_msg(-1.0, 0.0, 0.0)) + node._on_uav(pose_msg(1.0, 0.0, 1.0)) + node._tick() + start = node._length + assert node._state.obj == pytest.approx((3.0, 0.0)) + assert node._state.taut # slack 0: taut from the start + + node._on_rate(Float64(data=-5.0)) # clamped to max_reel_speed 1.0 + node._tick() + assert node._length == pytest.approx(start - 0.1) # 1 m/s at 10 Hz + assert 1.0 < node._state.obj[0] < 3.0 + assert not node._reached + + for _ in range(30): + node._tick() + assert node._state.obj == pytest.approx((1.0, 0.0)) # under the UAV, never lifted + assert node._reached + finally: + node.destroy_node() + + +def test_tether_sim_object_jams_on_a_pillar(rclpy_context, scenario_file): + from std_msgs.msg import Float64 + from hero_sim.tether_sim import TetherSim + + # A pillar between the object (3, 0) and the point under the UAV (1, 0). + scenario_file(TETHER_SCENARIO + 'obstacles: [{x: 2.0, y: 0.0, radius: 0.2}]\n') + node = TetherSim() + try: + node._on_ugv(pose_msg(-1.0, 0.0, 0.0)) + node._on_uav(pose_msg(1.0, 0.0, 1.0)) + node._on_rate(Float64(data=-1.0)) + for _ in range(30): + node._tick() + assert node._state.stuck and not node._reached + assert 2.3 <= node._state.obj[0] <= 2.35 # pillar 0.2 + object 0.1 + finally: + node.destroy_node() + + +def test_tether_sim_needs_a_tether_section(make): + from hero_sim.tether_sim import TetherSim + with pytest.raises(RuntimeError, match='no tether'): + make(TetherSim) diff --git a/test/test_obstacles.py b/test/test_obstacles.py new file mode 100644 index 0000000..ab2cbee --- /dev/null +++ b/test/test_obstacles.py @@ -0,0 +1,211 @@ +import math + +import numpy as np +import pytest + +from conftest import CONFIG_DIR, wall_map +from hero_sim import config +from hero_sim.config import Pillar +from hero_sim.obstacles import ( + _KEY_ORIGIN, PILLAR_RES, ObstacleMap, load, pillar_voxels, read_bt, write_bt, + write_pillar_map, +) + +MAPS = sorted((CONFIG_DIR.parent / 'maps').glob('*.bt')) +ICUAS26_1 = CONFIG_DIR.parent / 'maps' / 'icuas26_1.bt' +TRUNK = (-3.0, 0.15) # a tree trunk in icuas26_1 + + +def pillar_map(max_radius=0.5): + """No map grid, one pillar of radius 0.2 at (1, 0).""" + return ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, max_radius, [(1.0, 0.0, 0.2)]) + + +@pytest.mark.parametrize('path', MAPS, ids=lambda p: p.name) +def test_read_bt_shipped_maps(path): + res, boxes = read_bt(path) # raises on trailing bytes, so the whole tree was read + assert res > 0 and len(boxes) > 0 + assert set(np.unique(boxes[:, 3])) <= {1, 2} + + +def test_read_bt_rejects_other_files(tmp_path): + path = tmp_path / 'm.bt' + path.write_bytes(b'# not an octomap\n') + with pytest.raises(ValueError, match='not an octomap'): + read_bt(path) + + +def test_band_keeps_trunks_and_drops_floor(): + trunks = ObstacleMap.from_bt(ICUAS26_1, 0.2, 2.0, max_radius=0.5) + assert trunks.clearance(*TRUNK) == 0.0 + assert trunks.clearance(0.0, 0.0) == math.inf + floor = ObstacleMap.from_bt(ICUAS26_1, 0.0, 0.1, max_radius=0.5) + assert floor.clearance(0.0, 0.0) == 0.0 + + +def test_empty_band_has_no_obstacles(): + empty = ObstacleMap.from_bt(ICUAS26_1, 10.0, 11.0, max_radius=0.5) + assert empty.clearance(*TRUNK) == math.inf + + +def test_clearance_measured_to_the_obstacle_edge(): + m = wall_map() + # From the centre of the cell holding the point to the wall face at x = 1.0. + assert m.clearance(1.05, 0.0) == 0.0 + assert m.clearance(0.95, 0.0) == pytest.approx(0.05) + assert m.clearance(0.82, 0.0) == pytest.approx(0.15) + assert m.clearance(0.4, 0.0) == math.inf # beyond max_radius + + +def test_move_stops_before_the_wall_by_radius(): + m = wall_map() + x, y = m.move(0.0, 0.0, 2.0, 0.0, radius=0.3) + assert y == 0.0 + assert 0.6 <= x <= 0.75 # wall face at 1.0, minus the radius, within a cell + + +def test_wider_robot_stops_further_out(): + m = wall_map() + narrow, _ = m.move(0.0, 0.0, 2.0, 0.0, radius=0.1) + wide, _ = m.move(0.0, 0.0, 2.0, 0.0, radius=0.4) + assert wide < narrow - 0.2 + + +def test_fast_move_cannot_tunnel_through(): + m = wall_map() + # One step that would land well clear on the far side. + x, _ = m.move(0.5, 0.0, 1.8, 0.0, radius=0.2) + assert x < 1.0 + + +def test_free_moves_untouched(): + m = wall_map() + assert m.move(0.0, 0.0, 0.3, 0.2, radius=0.3) == (0.3, 0.2) + assert m.move(0.0, -2.0, 3.0, -2.0, radius=0.3) == (3.0, -2.0) # passes below the wall + + +def test_overlapping_robot_can_back_out_but_not_go_deeper(): + m = wall_map() + assert m.move(0.95, 0.0, 0.5, 0.0, radius=0.3) == (0.5, 0.0) + x, _ = m.move(0.95, 0.0, 1.05, 0.0, radius=0.3) + assert x < 1.0 + + +def test_radius_zero_never_collides_and_too_big_raises(): + m = wall_map(max_radius=0.3) + assert m.move(0.0, 0.0, 2.0, 0.0, radius=0.0) == (2.0, 0.0) + with pytest.raises(ValueError, match='max_radius'): + m.move(0.0, 0.0, 1.0, 0.0, radius=0.4) + + +def test_load_off_without_robots_or_collision(): + sim = dict(config.SIM_DEFAULTS, map=str(ICUAS26_1)) + assert load(sim, [0.1]) is not None + assert load(dict(sim, collision=False), [0.1]) is None + assert load(sim, []) is None + + +def test_load_without_map_or_pillars_still_checks_others(): + m = load(dict(config.SIM_DEFAULTS, map=None), [0.3]) + assert m.clearance(0.0, 0.0) == math.inf + assert m.move(0.0, 0.0, 2.0, 0.0, 0.3) == (2.0, 0.0) + x, _ = m.move(0.0, 0.0, 2.0, 0.0, 0.3, others=[(1.0, 0.0, 0.2)]) + assert 0.5 - PILLAR_RES / 2 <= x <= 0.5 + + +def test_pillar_clearance_is_exact_to_its_edge(): + m = pillar_map() + assert m.clearance(1.5, 0.0) == pytest.approx(0.3) + assert m.clearance(1.0, -0.45) == pytest.approx(0.25) + assert m.clearance(1.0, 0.1) == 0.0 # inside + assert m.clearance(2.0, 0.0) == math.inf # 0.8 m: beyond max_radius + + +def test_clearance_is_the_nearer_of_map_and_pillar(): + occupied = np.zeros((10, 2), bool) + occupied[:, 0] = True # wall_map's wall, face at x = 1.0 + m = ObstacleMap(occupied, 1.0, -0.5, 0.1, 0.5, [(0.3, 0.0, 0.1)]) + assert m.clearance(0.62, 0.0) == pytest.approx(0.22) # pillar edge at 0.4 + assert m.clearance(0.88, 0.0) == pytest.approx(0.15) # the wall, from the cell centre + + +def test_move_stops_before_a_pillar_by_radius(): + m = pillar_map() + x, y = m.move(0.0, 0.0, 2.0, 0.0, radius=0.3) + assert y == 0.0 + assert 0.5 - PILLAR_RES / 2 <= x <= 0.5 # pillar edge at 0.8, minus the radius + assert m.move(0.0, 0.6, 2.0, 0.6, radius=0.3) == (2.0, 0.6) # passes beside it + + +def test_others_count_like_pillars(): + m = ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, 0.5) + robot = [(1.0, 0.0, 0.2)] + assert m.clearance(1.5, 0.0) == math.inf + assert m.clearance(1.5, 0.0, robot) == pytest.approx(0.3) + x, y = m.move(0.0, 0.0, 2.0, 0.0, 0.3, robot) + assert 0.5 - PILLAR_RES / 2 <= x <= 0.5 and y == 0.0 + assert m.move(0.0, 0.6, 2.0, 0.6, 0.3, robot) == (2.0, 0.6) # passes beside it + assert m.move(0.7, 0.0, 0.0, 0.0, 0.3, robot) == (0.0, 0.0) # overlapping: backs away + assert m.move(0.7, 0.0, 0.9, 0.0, 0.3, robot) == (0.7, 0.0) # but not deeper + + +def test_nearest_of_pillars_and_others(): + m = pillar_map() # pillar edge at x = 0.8 + assert m.clearance(0.0, 0.0, [(0.0, 0.5, 0.1)]) == pytest.approx(0.4) + assert m.clearance(0.0, 0.0, [(0.0, 2.0, 0.1)]) == math.inf # both beyond max_radius + + +def test_load_pillars_with_and_without_map(): + pillars = [(1.0, 0.0, 0.2)] + sim = dict(config.SIM_DEFAULTS, map=None) + m = load(sim, [0.1], pillars) + assert m.clearance(1.0, 0.0) == 0.0 and m.res == PILLAR_RES + assert load(dict(sim, collision=False), [0.1], pillars) is None + assert load(sim, [], pillars) is None + + both = load(dict(sim, map=str(ICUAS26_1)), [0.1], [(0.0, 0.0, 0.2)]) + assert both.clearance(0.0, 0.0) == 0.0 + assert both.clearance(*TRUNK) == 0.0 + + +def bt_header(path): + head = path.read_bytes().split(b'data\n')[0].decode() + return dict(line.split(None, 1) for line in head.splitlines() if not line.startswith('#')) + + +def test_write_bt_one_voxel(tmp_path): + path = tmp_path / 'one.bt' + write_bt(path, {(_KEY_ORIGIN, _KEY_ORIGIN + 1, _KEY_ORIGIN + 2)}, 0.05) + # octomap checks the node count: the root plus one node on each of 16 levels. + assert bt_header(path) == {'id': 'OcTree', 'size': '17', 'res': '0.05'} + res, boxes = read_bt(path) + assert res == 0.05 + assert boxes.tolist() == [[_KEY_ORIGIN, _KEY_ORIGIN + 1, _KEY_ORIGIN + 2, 1]] + with pytest.raises(ValueError, match='no voxels'): + write_bt(path, set(), 0.05) + + +def test_pillar_voxels_fill_the_circle_from_the_floor_up(): + voxels = pillar_voxels([Pillar(1.0, -0.5, 0.2, height=0.5)], res=0.05) + assert all(math.hypot((kx - _KEY_ORIGIN + 0.5) * 0.05 - 1.0, + (ky - _KEY_ORIGIN + 0.5) * 0.05 + 0.5) <= 0.2 for kx, ky, _ in voxels) + assert sorted({kz - _KEY_ORIGIN for _, _, kz in voxels}) == list(range(10)) # 0 to 0.5 m + columns = len(voxels) // 10 + assert columns == pytest.approx(math.pi * 0.2 ** 2 / 0.05 ** 2, rel=0.1) + + +def test_pillar_map_reads_back_as_the_same_obstacle(tmp_path): + pillars = [Pillar(1.5, 0.5, 0.35), Pillar(-1.0, 2.0, 0.2, height=1.0)] + path = write_pillar_map(tmp_path / 'new' / 'p.bt', pillars) + res, boxes = read_bt(path) + assert {tuple(b[:3]) for b in boxes.tolist()} == pillar_voxels(pillars) + flat = ObstacleMap.from_bt(path, 0.2, 2.0, max_radius=0.5) + exact = ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, 0.5, pillars) + for x, y in ((1.5, 0.5), (-1.0, 2.0), (1.5 + 0.45, 0.5), (-1.0, 2.0 - 0.3), (0.0, 0.0)): + assert flat.clearance(x, y) == pytest.approx(exact.clearance(x, y), abs=res) + + +def test_circles_ignore_a_pillar_height(): + a = ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, 0.5, [(1.0, 0.0, 0.2)]) + b = ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, 0.5, [Pillar(1.0, 0.0, 0.2, 0.1)]) + assert a.circles == b.circles == ((1.0, 0.0, 0.2),) diff --git a/test/test_startup.py b/test/test_startup.py new file mode 100644 index 0000000..d1a9600 --- /dev/null +++ b/test/test_startup.py @@ -0,0 +1,63 @@ +"""startup/_setup.sh finds configs by the same rules as config.find_config. Needs bash.""" +from pathlib import Path +import shutil +import subprocess + +import pytest + +SETUP = Path(__file__).resolve().parent.parent / 'startup' / '_setup.sh' + +pytestmark = pytest.mark.skipif(shutil.which('bash') is None, reason='needs bash') + + +@pytest.fixture() +def tree(tmp_path): + """A copy of _setup.sh in startup/, next to a config/ like the repo's, default b.yaml.""" + (tmp_path / 'startup').mkdir() + text = SETUP.read_text() + default = next(line for line in text.splitlines() if line.startswith('DEFAULT_CONFIG_NAME=')) + (tmp_path / 'startup' / '_setup.sh').write_text( + text.replace(default, 'DEFAULT_CONFIG_NAME="b.yaml"')) + for rel in ('config/a.yaml', 'config/tether/b.yaml', 'config/mrs/dup.yaml', + 'config/tether/dup.yaml', 'here/c.yaml'): + (tmp_path / rel).parent.mkdir(parents=True, exist_ok=True) + (tmp_path / rel).write_text('{}\n') + return tmp_path + + +def source_setup(tree, *args): + """Source _setup.sh from `tree` with args. Returns (exit code, HERO_SIM_CONFIG, stderr).""" + script = 'source startup/_setup.sh "$@" > /dev/null && printf %s "$HERO_SIM_CONFIG"' + done = subprocess.run(['bash', '-c', script, 'bash', *args], cwd=tree, + capture_output=True, text=True, + env={'PATH': '/usr/bin:/bin', 'HOME': str(tree)}) + return done.returncode, done.stdout, done.stderr + + +@pytest.mark.parametrize('arg, found', [ + ('a.yaml', 'config/a.yaml'), # on top of config/ + ('b.yaml', 'config/tether/b.yaml'), # in a folder, by name + ('tether/b.yaml', 'config/tether/b.yaml'), + ('mrs/dup.yaml', 'config/mrs/dup.yaml'), + ('here/c.yaml', 'here/c.yaml'), # an existing path, relative to where it runs +]) +def test_setup_finds_configs(tree, arg, found): + code, path, err = source_setup(tree, arg) + assert code == 0, err + assert path == str(tree / found) + + +def test_setup_defaults_and_absolute_paths(tree): + assert source_setup(tree)[1] == str(tree / 'config' / 'tether' / 'b.yaml') + assert source_setup(tree, str(tree / 'here' / 'c.yaml'))[1] == str(tree / 'here' / 'c.yaml') + + +@pytest.mark.parametrize('arg, message', [ + ('dup.yaml', 'more than one config folder'), + ('nope.yaml', 'config not found'), + ('mrs/b.yaml', 'config not found'), + ('/no/such/file.yaml', 'config not found: /no/such/file.yaml'), +]) +def test_setup_errors(tree, arg, message): + code, _, err = source_setup(tree, arg) + assert code != 0 and message in err diff --git a/test/test_tether.py b/test/test_tether.py new file mode 100644 index 0000000..0c7a267 --- /dev/null +++ b/test/test_tether.py @@ -0,0 +1,85 @@ +import math + +import numpy as np +import pytest + +from conftest import wall_map +from hero_sim.obstacles import PILLAR_RES, ObstacleMap +from hero_sim.tether import initial_length, reached, step + +# Winch, UGV pulley and UAV ring on the x axis, UAV 1 m up, object tied at the floor. +WINCH, UGV, UAV = (-2.0, 0.0, 0.0), (-1.0, 0.0, 0.0), (1.0, 0.0, 1.0) +ROUTED = 1.0 + math.sqrt(5.0) # |W - G| + |G - A| +OBJ = (3.0, 0.0) # |A - O| = sqrt(5) +# A pillar between the object and the point under the UAV. +PILLAR = ObstacleMap(np.zeros((0, 0), bool), 0.0, 0.0, PILLAR_RES, 0.5, [(2.0, 0.0, 0.2)]) + + +def test_initial_length_leaves_the_slack(): + assert initial_length(WINCH, UGV, UAV, OBJ, 0.0, 0.5) == pytest.approx( + ROUTED + math.sqrt(5.0) + 0.5) + + +def test_slack_tether_leaves_the_object(): + s = step(ROUTED + math.sqrt(5.0) + 0.5, WINCH, UGV, UAV, OBJ, 0.0) + assert s.obj == OBJ + assert s.free == pytest.approx(math.sqrt(5.0) + 0.5) + assert not s.taut and not s.stuck + + +def test_taut_tether_slides_object_toward_the_point_under_the_uav(): + s = step(ROUTED + 1.5, WINCH, UGV, UAV, OBJ, 0.0) + assert s.obj == pytest.approx((1.0 + math.sqrt(1.5 ** 2 - 1.0), 0.0)) + assert math.dist(UAV, (*s.obj, 0.0)) == pytest.approx(1.5) + assert s.taut and not s.stuck + + +def test_object_is_never_lifted_the_winch_slips(): + s = step(ROUTED + 0.2, WINCH, UGV, UAV, OBJ, 0.0) # less than the UAV's height + assert s.length == pytest.approx(ROUTED + 1.0) + assert s.obj == pytest.approx((1.0, 0.0)) + assert s.free == pytest.approx(1.0) + + +def test_attach_height_shortens_the_drop(): + s = step(0.0, WINCH, UGV, UAV, OBJ, 0.25) + assert s.free == pytest.approx(0.75) + + +def test_object_jams_against_a_pillar_and_the_winch_slips(): + s = step(ROUTED + 1.5, WINCH, UGV, UAV, OBJ, 0.0, PILLAR, 0.1) + # Touching: pillar 0.2 + object 0.1, stopped within a path sample. + assert 2.3 <= s.obj[0] <= 2.3 + PILLAR_RES / 2 and s.obj[1] == 0.0 + assert s.stuck and s.taut + assert s.free == pytest.approx(math.dist(UAV, (*s.obj, 0.0))) + assert s.length == pytest.approx(ROUTED + s.free) + + again = step(s.length - 0.5, WINCH, UGV, UAV, s.obj, 0.0, PILLAR, 0.1) + assert again.obj == s.obj and again.stuck + + +def test_pull_away_from_the_pillar_frees_the_object(): + uav = (4.0, 0.0, 1.0) # now on the far side of the object from the pillar + s = step(0.0, WINCH, UGV, uav, (2.3, 0.0), 0.0, PILLAR, 0.1) + assert s.obj == pytest.approx((4.0, 0.0)) + assert not s.stuck + + +def test_pillar_beside_the_path_lets_it_through(): + s = step(ROUTED + 1.5, WINCH, UGV, UAV, (3.0, 1.0), 0.0, PILLAR, 0.1) # ends at (2, 0.5) + assert not s.stuck + assert math.dist(UAV, (*s.obj, 0.0)) == pytest.approx(1.5) + + +def test_object_jams_against_the_map(): + # Pulled from x = 2 toward x ~ 0.66 through wall_map's wall at x = 1.0..1.1. + uav = (0.0, 0.0, 1.0) + routed = math.dist(WINCH, UGV) + math.dist(UGV, uav) + s = step(routed + 1.2, WINCH, UGV, uav, (2.0, 0.0), 0.0, wall_map(), 0.1) + assert s.stuck + assert 1.2 - 0.05 <= s.obj[0] <= 1.2 + 0.1 # far face + radius, within a cell + + +def test_reached(): + assert reached((1.0, 1.0), (1.0, 1.3, 0.4)) + assert not reached((1.0, 1.0), (1.0, 1.5, 0.4))