A place for spot tools

python3 -m venv spot_tools_env
pip install -e .

To run the linter on everything:

pre-commit run --all-files

You can find an example of the ROS-free spot executor in examples/test_spot_executor.py. You should be able to run this and see 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.

You can find the implemented mid-level planner in robot_executor_interface/src/robot_executor_interface/mid_level_planner.py.

The mid-level planner can be tested using the DCIST launch system with fake publishers to simulate robot behavior and occupancy data.

First, launch the complete DCIST system in simulation mode:

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.

In a separate terminal, launch the fake occupancy publisher to provide simulated occupancy grid data:

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.

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:

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.

In another terminal, use the fake path publisher to send waypoint commands to the planner:

ros2 run spot_tools_ros fake_path_publisher <x> <y>

Where <x> and <y> are the desired target coordinates. For example:

ros2 run spot_tools_ros fake_path_publisher -6 1

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

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.