Collisions, pillars and their octomap, tether task, ground go_to, config folders, student README

.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

README.md

@@ -1,114 +1,209 @@
-
 # 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.
 
 <p align="center">
   <img src="media/rviz2.png" width="700">
 </p>
 
 ## 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/<scenario>_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 `/<name>/pose` (`geometry_msgs/PoseStamped`, in the `world` frame) and on TF.
 
-The meshes in `models/<name>/` 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 | |
+|---|---|---|
+| `/<name>/cmd_vel` | topic, `geometry_msgs/Twist` | velocity in the robot's own frame. Kept until the next message, so one is enough |
+| `/<name>/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.
+
+<p align="center">
+  <img src="media/tether_demo.png" width="800">
+</p>
 
 ```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 `/<name>/pose` and as TF from `world`.
+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).

config/example.yaml

@@ -3,10 +3,19 @@
 
 # Kinematic ground robots. Each one subscribes to /<name>/cmd_vel (body-frame
 # Twist, held until the next message) and publishes /<name>/pose plus TF
-# world -> <name> at z = 0.
+# world -> <name> 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 /<name>/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/<cf>/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/<name>.bt, a path to a .bt, or null for none
+  # map: icuas26_1           # octomap: maps/<name>.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