|
| 1 | +<!-- |
| 2 | +SPDX-FileCopyrightText: 2025 James Harton |
| 3 | +
|
| 4 | +SPDX-License-Identifier: Apache-2.0 |
| 5 | +--> |
| 6 | + |
| 7 | +# How to Add a Custom Command |
| 8 | + |
| 9 | +Create a command handler that integrates with the robot state machine and provides structured feedback. |
| 10 | + |
| 11 | +## Prerequisites |
| 12 | + |
| 13 | +- Familiarity with the BB DSL (see [First Robot](../tutorials/01-first-robot.md)) |
| 14 | +- Understanding of the command system (see [Commands and State Machine](../tutorials/05-commands.md)) |
| 15 | + |
| 16 | +## Step 1: Define the Command in DSL |
| 17 | + |
| 18 | +Add the command to your robot's `commands` block: |
| 19 | + |
| 20 | +```elixir |
| 21 | +defmodule MyRobot do |
| 22 | + use BB |
| 23 | + |
| 24 | + commands do |
| 25 | + command :arm do |
| 26 | + handler BB.Command.Arm |
| 27 | + allowed_states [:disarmed] |
| 28 | + end |
| 29 | + |
| 30 | + command :disarm do |
| 31 | + handler BB.Command.Disarm |
| 32 | + allowed_states [:idle] |
| 33 | + end |
| 34 | + |
| 35 | + command :move_to do |
| 36 | + handler MyRobot.MoveToCommand |
| 37 | + allowed_states [:idle] |
| 38 | + |
| 39 | + argument :target, {:map, :atom, :float} do |
| 40 | + required true |
| 41 | + doc "Target joint positions in radians" |
| 42 | + end |
| 43 | + |
| 44 | + argument :velocity, :float do |
| 45 | + required false |
| 46 | + default 1.0 |
| 47 | + doc "Movement velocity multiplier" |
| 48 | + end |
| 49 | + end |
| 50 | + end |
| 51 | + |
| 52 | + topology do |
| 53 | + # ... your robot topology |
| 54 | + end |
| 55 | +end |
| 56 | +``` |
| 57 | + |
| 58 | +## Step 2: Create the Handler Module |
| 59 | + |
| 60 | +Create a module using `BB.Command`: |
| 61 | + |
| 62 | +```elixir |
| 63 | +defmodule MyRobot.MoveToCommand do |
| 64 | + use BB.Command |
| 65 | + |
| 66 | + alias BB.Message.Sensor.JointCommand |
| 67 | + alias BB.PubSub |
| 68 | + |
| 69 | + @impl BB.Command |
| 70 | + def handle_command(goal, context, state) do |
| 71 | + target = Map.fetch!(goal, :target) |
| 72 | + velocity = Map.get(goal, :velocity, 1.0) |
| 73 | + |
| 74 | + # Subscribe to sensor feedback |
| 75 | + for {joint_name, _position} <- target do |
| 76 | + PubSub.subscribe(context.robot_module, [:sensor, joint_name]) |
| 77 | + end |
| 78 | + |
| 79 | + # Send commands to actuators |
| 80 | + for {joint_name, position} <- target do |
| 81 | + command = JointCommand.new!(name: joint_name, target: position) |
| 82 | + PubSub.publish(context.robot_module, [:actuator, joint_name], command) |
| 83 | + end |
| 84 | + |
| 85 | + # Store target and wait for completion |
| 86 | + {:noreply, Map.merge(state, %{ |
| 87 | + target: target, |
| 88 | + velocity: velocity, |
| 89 | + positions: %{} |
| 90 | + })} |
| 91 | + end |
| 92 | + |
| 93 | + @impl BB.Command |
| 94 | + def handle_info({:bb, [:sensor, joint_name], %{payload: joint_state}}, state) do |
| 95 | + current = hd(joint_state.positions) |
| 96 | + target = Map.get(state.target, joint_name) |
| 97 | + |
| 98 | + if target && close_enough?(current, target) do |
| 99 | + new_positions = Map.put(state.positions, joint_name, current) |
| 100 | + |
| 101 | + if all_complete?(new_positions, state.target) do |
| 102 | + {:stop, :normal, %{state | result: {:ok, new_positions}}} |
| 103 | + else |
| 104 | + {:noreply, %{state | positions: new_positions}} |
| 105 | + end |
| 106 | + else |
| 107 | + {:noreply, state} |
| 108 | + end |
| 109 | + end |
| 110 | + |
| 111 | + def handle_info(_msg, state), do: {:noreply, state} |
| 112 | + |
| 113 | + @impl BB.Command |
| 114 | + def result(%{result: result}), do: result |
| 115 | + |
| 116 | + defp close_enough?(current, target), do: abs(current - target) < 0.01 |
| 117 | + |
| 118 | + defp all_complete?(positions, target) do |
| 119 | + Enum.all?(target, fn {joint, _} -> Map.has_key?(positions, joint) end) |
| 120 | + end |
| 121 | +end |
| 122 | +``` |
| 123 | + |
| 124 | +## Step 3: Use the Command |
| 125 | + |
| 126 | +The DSL generates a convenience function on your robot module: |
| 127 | + |
| 128 | +```elixir |
| 129 | +# Start the robot |
| 130 | +{:ok, _} = BB.Supervisor.start_link(MyRobot) |
| 131 | + |
| 132 | +# Arm first (commands need :idle state) |
| 133 | +{:ok, cmd} = MyRobot.arm() |
| 134 | +{:ok, :armed, _} = BB.Command.await(cmd) |
| 135 | + |
| 136 | +# Execute your command |
| 137 | +{:ok, cmd} = MyRobot.move_to(target: %{shoulder: 0.5, elbow: 1.0}) |
| 138 | + |
| 139 | +# Wait for completion |
| 140 | +case BB.Command.await(cmd, 10_000) do |
| 141 | + {:ok, positions} -> |
| 142 | + IO.puts("Moved to: #{inspect(positions)}") |
| 143 | + |
| 144 | + {:error, reason} -> |
| 145 | + IO.puts("Movement failed: #{inspect(reason)}") |
| 146 | +end |
| 147 | +``` |
| 148 | + |
| 149 | +## Adding Timeout Handling |
| 150 | + |
| 151 | +For commands that might hang, add timeout logic: |
| 152 | + |
| 153 | +```elixir |
| 154 | +defmodule MyRobot.MoveToCommand do |
| 155 | + use BB.Command |
| 156 | + |
| 157 | + @timeout_ms 5000 |
| 158 | + |
| 159 | + @impl BB.Command |
| 160 | + def handle_command(goal, context, state) do |
| 161 | + # ... setup code ... |
| 162 | + |
| 163 | + # Schedule timeout |
| 164 | + timer_ref = Process.send_after(self(), :timeout, @timeout_ms) |
| 165 | + |
| 166 | + {:noreply, Map.put(state, :timer_ref, timer_ref)} |
| 167 | + end |
| 168 | + |
| 169 | + @impl BB.Command |
| 170 | + def handle_info(:timeout, state) do |
| 171 | + {:stop, :normal, %{state | result: {:error, :timeout}}} |
| 172 | + end |
| 173 | + |
| 174 | + def handle_info({:bb, [:sensor, _], _} = msg, state) do |
| 175 | + # Cancel timeout on any progress |
| 176 | + if state[:timer_ref] do |
| 177 | + Process.cancel_timer(state.timer_ref) |
| 178 | + end |
| 179 | + |
| 180 | + # ... existing sensor handling ... |
| 181 | + |
| 182 | + {:noreply, Map.put(state, :timer_ref, new_timer_ref)} |
| 183 | + end |
| 184 | +end |
| 185 | +``` |
| 186 | + |
| 187 | +## Handling Safety State Changes |
| 188 | + |
| 189 | +React to safety transitions during execution: |
| 190 | + |
| 191 | +```elixir |
| 192 | +@impl BB.Command |
| 193 | +def handle_safety_state_change(:disarming, state) do |
| 194 | + # Robot is being disarmed - stop gracefully |
| 195 | + {:stop, :disarmed, %{state | result: {:error, :disarmed}}} |
| 196 | +end |
| 197 | + |
| 198 | +def handle_safety_state_change(_new_state, state) do |
| 199 | + # Continue execution (use with care!) |
| 200 | + {:continue, state} |
| 201 | +end |
| 202 | +``` |
| 203 | + |
| 204 | +The default implementation stops with `:disarmed` on any safety state change. |
| 205 | + |
| 206 | +## Command Cancellation |
| 207 | + |
| 208 | +Allow your command to be cancelled by other commands: |
| 209 | + |
| 210 | +```elixir |
| 211 | +command :move_to do |
| 212 | + handler MyRobot.MoveToCommand |
| 213 | + allowed_states [:idle] |
| 214 | + cancel [:default] # Can be cancelled by other :default commands |
| 215 | +end |
| 216 | + |
| 217 | +command :emergency_stop do |
| 218 | + handler MyRobot.EmergencyStopCommand |
| 219 | + allowed_states :* # Run in any state |
| 220 | + cancel :* # Cancel all running commands |
| 221 | +end |
| 222 | +``` |
| 223 | + |
| 224 | +When cancelled, awaiting callers receive `{:error, :cancelled}`. |
| 225 | + |
| 226 | +## State Transitions |
| 227 | + |
| 228 | +Commands can transition the robot to a new state: |
| 229 | + |
| 230 | +```elixir |
| 231 | +@impl BB.Command |
| 232 | +def result(%{result: {:ok, value}, next_state: next_state}) do |
| 233 | + {:ok, value, next_state: next_state} |
| 234 | +end |
| 235 | +``` |
| 236 | + |
| 237 | +This is how `BB.Command.Arm` and `BB.Command.Disarm` work - they set `next_state` to `:idle` and `:disarmed` respectively. |
| 238 | + |
| 239 | +## Structured Errors |
| 240 | + |
| 241 | +Return structured errors from `BB.Error`: |
| 242 | + |
| 243 | +```elixir |
| 244 | +alias BB.Error.State.NotAllowed |
| 245 | + |
| 246 | +@impl BB.Command |
| 247 | +def handle_command(goal, context, state) do |
| 248 | + case validate_goal(goal, context) do |
| 249 | + :ok -> |
| 250 | + # proceed |
| 251 | + {:noreply, state} |
| 252 | + |
| 253 | + {:error, reason} -> |
| 254 | + {:stop, :normal, %{state | result: {:error, reason}}} |
| 255 | + end |
| 256 | +end |
| 257 | + |
| 258 | +defp validate_goal(goal, context) do |
| 259 | + target = goal[:target] || %{} |
| 260 | + joints = Map.keys(context.robot.joints) |
| 261 | + |
| 262 | + invalid = Map.keys(target) -- joints |
| 263 | + if invalid == [] do |
| 264 | + :ok |
| 265 | + else |
| 266 | + {:error, BB.Error.Invalid.UnknownJoints.exception(joints: invalid)} |
| 267 | + end |
| 268 | +end |
| 269 | +``` |
| 270 | + |
| 271 | +## Testing Commands |
| 272 | + |
| 273 | +Test command handlers with the robot in simulation mode: |
| 274 | + |
| 275 | +```elixir |
| 276 | +defmodule MyRobot.MoveToCommandTest do |
| 277 | + use ExUnit.Case |
| 278 | + |
| 279 | + setup do |
| 280 | + {:ok, _} = BB.Supervisor.start_link(MyRobot, simulation: :kinematic) |
| 281 | + {:ok, cmd} = MyRobot.arm() |
| 282 | + {:ok, :armed, _} = BB.Command.await(cmd) |
| 283 | + :ok |
| 284 | + end |
| 285 | + |
| 286 | + test "moves to target positions" do |
| 287 | + {:ok, cmd} = MyRobot.move_to(target: %{shoulder: 0.5}) |
| 288 | + assert {:ok, %{shoulder: position}} = BB.Command.await(cmd, 5000) |
| 289 | + assert_in_delta position, 0.5, 0.02 |
| 290 | + end |
| 291 | + |
| 292 | + test "returns error for invalid joints" do |
| 293 | + {:ok, cmd} = MyRobot.move_to(target: %{nonexistent: 0.5}) |
| 294 | + assert {:error, %BB.Error.Invalid.UnknownJoints{}} = BB.Command.await(cmd) |
| 295 | + end |
| 296 | +end |
| 297 | +``` |
| 298 | + |
| 299 | +## Common Issues |
| 300 | + |
| 301 | +### Command not starting |
| 302 | + |
| 303 | +Check that: |
| 304 | +- The robot is in one of the `allowed_states` for the command |
| 305 | +- The command handler module is compiled and available |
| 306 | + |
| 307 | +### Command hangs forever |
| 308 | + |
| 309 | +Ensure you: |
| 310 | +- Call `{:stop, reason, state}` when complete |
| 311 | +- Handle timeout cases |
| 312 | +- Subscribe to the correct PubSub paths for feedback |
| 313 | + |
| 314 | +### State transition not working |
| 315 | + |
| 316 | +The `result/1` callback must return `{:ok, value, next_state: state}` - the third element must be a keyword list with `:next_state`. |
| 317 | + |
| 318 | +## Next Steps |
| 319 | + |
| 320 | +- Learn about [Custom States and Command Categories](../tutorials/11-custom-states.md) for advanced state machines |
| 321 | +- Understand the [Command System](../topics/command-system.md) architecture |
0 commit comments