Spot Tools

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

Examples

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.

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:

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:

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:

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:

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

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.

Operator approval and walk-to during Pick

Every Pick (and Drag) grasp is reviewed by an operator. The executor publishes a ManipulationApprovalRequest with one image per camera on ~/manipulation_request and blocks until a ManipulationApprovalResponse arrives on ~/pick_confirmation. The response has three outcomes:

Response	Robot behaviour
approve=true	Grasp at the selected pixel in the selected image.
approve=false	Abandon this grasp attempt (the executor may retry the action).
approve=false, walk_to=true	Stow the arm, walk toward the selected ground/object pixel, capture new images and ask again. Repeats until approve/reject, within one grasp attempt.

Walk-to lifts the pixel onto Spot's ground plane estimate using the image's pinhole model and transform snapshot (spot_skills/walk_to_utils.py), then drives to a standoff pose facing that point. The standoff distance is the executor parameter pick_walk_to_standoff (default 1.5 m, from the body center; Spot's front is ~0.55 m ahead of center). If the response carries a stand point (has_stand_point, stand_x, stand_y), the robot stands on that ground pixel instead and faces the target from there, which gives the operator heading control; the operator UIs keep that option behind a flag that is off by default.

Pausing or terminating the plan interrupts a walk-to like any other motion. The offline tests spot_tools/examples/test_walk_to_geometry.py and spot_tools/examples/test_object_grasp_walk_to.py cover the geometry and the approval loop without a robot or the Boston Dynamics SDK.

Holding updates over relayed links

The executor retries the base station's UpdateHoldingState service using holding_service_attempts (default 3), holding_service_wait_s (3 seconds per attempt), and holding_service_call_timeout_s (5 seconds per reply). A failed update is logged without changing a successful pick/place result; /action_done also reports the completed action. The client uses a separate callback group so a preempting plan cannot block its reply while waiting for the old sequence.