robot WarehouseBot {
  node logistics on "/warehouse";

  topic status: String publish on "/status";

  service clear_map: ClearCostmap;
  service set_pose: SetPose;
  action follow_route: FollowPath;

  sensor lidar: Lidar on "/scan";
  actuator wheels: DifferentialDrive;

  safety {
    max_speed = 0.8 m/s;
    zone loading_dock rect at (2.0 m, 1.0 m) size (1.5 m, 1.0 m);
    stop_if robot.in_zone("loading_dock");
  }

  behavior deliver() {
    let start = robot.pose();
    let dock = pose(x: 2.5 m, y: 1.5 m, theta: 1.57 rad);
    let path = trajectory(from: start, to: dock, steps: 10);
    let tf = transform(from: "base_link", to: "map", pose: start);

    call clear_map();
    call set_pose();
    send_goal follow_route with path;
    publish status with "en route";

    loop every 100ms {
      if robot.in_zone("loading_dock") {
        emergency_stop;
        wheels.stop();
      } else {
        reset_emergency_stop;
        wheels.follow(path: path);
      }
    }
  }
}
