Feature/mid level planner/main

README.md

@@ -19,3 +19,87 @@ a little plot of the agent moving along a path. Run it with `python -i` so
 that the plot stays at the end. If it crashes with an inscrutable error,
 you may need to `pip install opencv-python-headless` because of a conflict
 between opencv's QT version and matplotlib's QT version.
+
+
+# Testing Mid-Level Planner in Simulation
+
+You can find the implemented mid-level planner in `robot_executor_interface/src/robot_executor_interface/mid_level_planner.py`.
+
+## Setup and Testing Instructions
+
+The mid-level planner can be tested using the DCIST launch system with fake publishers to simulate robot behavior and occupancy data.
+
+### 1. Launch the DCIST System
+
+First, launch the complete DCIST system in simulation mode:
+
+```bash
+ADT4_SIM_TIME=false tmuxp load dcist_launch_system/tmux/autogenerated/spot_prior_dsg-spot_prior_dsg.yaml
+```
+
+This will start all the necessary nodes including the spot executor and planning systems.
+
+### 2. Start the Fake Occupancy Publisher
+
+In a separate terminal, launch the fake occupancy publisher to provide simulated occupancy grid data:
+
+```bash
+ros2 run spot_tools_ros fake_occupancy_publisher
+```
+
+This publishes a test occupancy grid that the mid-level planner uses for obstacle avoidance and path planning.
+
+#### Occupancy Publisher Parameters
+
+You can customize the occupancy grid simulation using the following parameters:
+
+- `--num_obstacles <N>`: Number of simulated obstacles in the occupancy grid (default: 5)
+- `--crop_distance <D>`: How far the robot can "see" in meters - areas beyond this distance are marked as unknown (default: 5.0, set to -1 to disable cropping)
+- `--resolution <R>`: Map resolution in meters per cell (default: 0.12)
+- `--robot_name <NAME>`: Robot name for topic namespacing (default: 'hamilton')
+- `--publish_rate <RATE>`: Publishing frequency in Hz (default: 10.0)
+
+**Example with custom parameters:**
+```bash
+ros2 run spot_tools_ros fake_occupancy_publisher --num_obstacles 10 --crop_distance 8.0
+```
+
+This creates a more challenging environment with 10 obstacles and allows the robot to "see" up to 8 meters away.
+
+### 3. Send Path Commands
+
+In another terminal, use the fake path publisher to send waypoint commands to the planner:
+
+```bash
+ros2 run spot_tools_ros fake_path_publisher <x> <y>
+```
+
+Where `<x>` and `<y>` are the desired target coordinates. For example:
+```bash
+ros2 run spot_tools_ros fake_path_publisher -6 1
+```
+
+### 4. Visualization in RViz
+
+Launch RViz to visualize the planning behavior and monitor the following key topics:
+
+1. **High-level Path**: `/hamilton/omniplanner_node/compiled_plan_viz_out`
+   - Shows the simulated high-level path from the omniplanner
+
+2. **Mid-Level Planner Output**: `/hamilton/spot_executor_node/mlp_path_publisher`
+   - Displays the locally planned path from the mid-level planner
+   - This path incorporates obstacle avoidance and local optimization
+
+3. **Occupancy Grid**: `/hamilton/hydra/tsdf/occupancy`
+   - Shows the TSDF occupancy map used for planning
+   - Visualizes obstacles and free space
+
+### Expected Behavior
+
+The mid-level planner should:
+- Receive high-level waypoints from the fake path publisher
+- Process the occupancy grid from the fake occupancy publisher
+- Generate locally optimal paths that avoid obstacles
+- Publish the resulting path for execution by the robot controller
+
+This testing setup allows you to validate the mid-level planner's obstacle avoidance, path smoothing, and local optimization capabilities in a controlled simulation environment.