documentation:robots:spot:spot_ros

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
Next revision		 Previous revision
documentation:robots:spot:spot_ros [2023/04/24 19:23] – added spot_ros topics and usage examples mingsdocumentation:robots:spot:spot_ros [2023/04/28 09:41] (current) – [Further Reading and Resources] mings
Line 17: Line 17:
      
 Build the package with `catkin build`. Build the package with `catkin build`.
-  cd ~/catkin_ws/src+  cd ~/catkin_ws
   catkin build   catkin build
      
Line 26: Line 26:
 Alternatively, you can make use of pre-built Docker images generated by the CI pipeline. Alternatively, you can make use of pre-built Docker images generated by the CI pipeline.
  
 +  - Download the image from Docker Hub with `sudo docker pull jeremysee/spot_ros:master`
 +  - Save the image into a .tar archive with `sudo docker save -o spotros.tar jeremysee/spot_ros:master`
 +  - Upload the .tar archive to Spot CORE using [[https://dev.bostondynamics.com/docs/payload/spot_core_portainer|this guide]]
 +
 +{{:documentation:robots:spot:portainer.png?800|}}
 ===== Configuration ===== ===== Configuration =====
  
Line 91: Line 96:
 ===== ROS Usage ===== ===== ROS Usage =====
  
-Here are some examples on how to make use of the spot_ros package in an external ROS node, to control the robot.+The documentation of available ROS services, topics and actions are listed below, explaining the capabilities of the spot_ros driver.
  
-==== Example 1 ====+===== ROS Services =====
  
-This was some example code used to traverse a GraphNav map and record fiducial positions along the way from the World Objects model of Spot. +The available ROS services and thier descriptions are in this section.
- +
-  #!/usr/bin/env python3 +
-  import typing +
-  import pickle +
-  import time +
-  import rospy +
-  import actionlib +
-  from actionlib_msgs.msg import GoalStatus +
-  from rosservice import get_service_class_by_name +
-  from std_msgs.msg import String +
-  from geometry_msgs.msg import PoseStamped +
-  from spot_msgs.msg import WorldObjectArray, WorldObject +
-  from spot_msgs.msg import AprilTagProperties +
-  from spot_msgs.msg import FrameTreeSnapshot, ParentEdge +
-  from spot_msgs.msg import NavigateInitAction, NavigateInitGoal +
-  from spot_msgs.msg import NavigateToAction, NavigateToGoal +
-  from spot_msgs.srv import ListGraphResponse +
-  from std_srvs.srv import TriggerRequest +
-  from fiducial import Fiducial +
-  class SpotNav: +
-      def __init__(self): +
-          self.world_objects = None +
-          self.fiducials_seen: typing.Dict[int, typing.List["Fiducial"]] = {} +
-          self.waypoint_fiducial: typing.Dict[str, typing.List["Fiducial"]] = {} +
-      def initialize_subscribers(self): +
-          """Initialize ROS subscribers""" +
-          # Create a subscriber for the /spot/world_objects topic for WorldObjectArray messages +
-          self.world_objects_sub = rospy.Subscriber( +
-              "/spot/world_objects", WorldObjectArray, self.world_objects_callback +
-          ) +
-      def initialize_publishers(self): +
-          """Initialize ROS publishers""" +
-          self.reached_waypoint_pub = rospy.Publisher( +
-              "/spot/nav/reached_waypoint", String, queue_size=1 +
-          ) +
-      def initialize_action_clients(self): +
-          """Initialize ROS action clients""" +
-          # Create an action client for the /spot/navigate_to action +
-          self.navigate_to_client = actionlib.SimpleActionClient( +
-              "/spot/navigate_to", NavigateToAction +
-          ) +
-          # Create an action client for the /spot/navigate_init action +
-          self.navigate_init_client = actionlib.SimpleActionClient( +
-              "/spot/navigate_init", NavigateInitAction +
-          ) +
-      def world_objects_callback(self, msg: WorldObjectArray): +
-          # Save the message to a class variable tracking the fiducials in the message +
-          self.world_objects: typing.List[WorldObject] = msg.world_objects +
-          # Iterate through the fiducials in the message, append the x,y,z coordinates to a dictionary +
-          for world_object in self.world_objects: +
-              april_tag: AprilTagProperties = world_object.apriltag_properties +
-              latest_snapshot: FrameTreeSnapshot = world_object.frame_tree_snapshot +
-              # Check if april_tag is None +
-              if april_tag is None: +
-                  continue +
-              # Create the FrameTreeSnapshot as a dictionary +
-              frame_tree_snapshot: typing.Dict[str, PoseStamped] = {} +
-              for child, parent_edge in zip( +
-                  latest_snapshot.child_edges, latest_snapshot.parent_edges +
-              ): +
-                  parent_edge_transform = PoseStamped() +
-                  parent_edge_transform.header.stamp = world_object.acquisition_time +
-                  parent_edge_transform.header.frame_id = parent_edge.parent_frame_name +
-                  parent_edge_transform.pose = parent_edge.parent_tform_child +
-                  frame_tree_snapshot[child] = parent_edge_transform +
-              april_tag_pose = frame_tree_snapshot[f"fiducial_{april_tag.tag_id}"] +
-              april_tag_pose_filtered = frame_tree_snapshot[ +
-                  f"filtered_fiducial_{april_tag.tag_id}" +
-              ] +
-              # Build the april_tag into the Fiducial class +
-              fiducial = Fiducial( +
-                  tag_id=april_tag.tag_id, +
-                  dim_x=april_tag.x, +
-                  dim_y=april_tag.y, +
-                  fiducial_pose=april_tag_pose, +
-                  filtered_fiducial_pose=april_tag_pose_filtered, +
-                  pose_covariance=april_tag.detection_covariance, +
-                  pose_covariance_frame=april_tag.detection_covariance_reference_frame, +
-              ) +
-              # Save the fiducial to the class variable +
-              if fiducial.tag_id in self.fiducials_seen: +
-                  self.fiducials_seen[fiducial.tag_id].append(fiducial) +
-              else: +
-                  self.fiducials_seen[fiducial.tag_id] = [fiducial] +
-              rospy.logdebug( +
-                  f"fiducial: {fiducial.tag_id}\n position_x: {fiducial.fiducial_pose.pose.position.x:.2f}\n filtered_position_x: {fiducial.filtered_fiducial_pose.pose.position.x:.2f}" +
-              ) +
-      def call_service(self, service_name: str, *args, **kwargs): +
-          """Call a service and wait for it to be available""" +
-          try: +
-              rospy.wait_for_service(service_name) +
-              service_type = get_service_class_by_name(service_name) +
-              proxy = rospy.ServiceProxy(service_name, service_type) +
-              return proxy(*args, **kwargs) +
-          except rospy.ServiceException as e: +
-              rospy.logerr("Service call failed: %s" % e) +
-      def walk_current_graph(self): +
-          """Walk the current graph in GraphNav""" +
-          rospy.loginfo("Walking the current graph") +
-          # Call the ListGraph service +
-          list_graph: ListGraphResponse = self.call_service("/spot/list_graph") +
-          waypoints = list_graph.waypoint_ids +
-          # Call the /spot/navigate_init action +
-          navigate_init_goal = NavigateInitGoal( +
-              upload_path="", +
-              initial_localization_fiducial=True, +
-              initial_localization_waypoint="mm", +
-          ) +
-          self.navigate_init_client.send_goal(navigate_init_goal) +
-          self.navigate_init_client.wait_for_result() +
-          # Check if the action succeeded +
-          if self.navigate_init_client.get_state() == GoalStatus.SUCCEEDED: +
-              rospy.loginfo("NavigateInit action succeeded") +
-          for waypoint in waypoints: +
-              # Call the /spot/navigate_to action +
-              navigate_to_goal = NavigateToGoal(navigate_to=waypoint) +
-              self.navigate_to_client.send_goal(navigate_to_goal) +
-              self.navigate_to_client.wait_for_result() +
-              # Check if the action succeeded +
-              if self.navigate_to_client.get_state() == GoalStatus.SUCCEEDED: +
-                  rospy.loginfo(f"NavigateTo {waypoint} action succeeded") +
-                  latest_world_objects_msg = rospy.wait_for_message( +
-                      "/spot/world_objects", WorldObjectArray +
-                  ) +
-                  latest_fiducial = self.extract_fiducials(latest_world_objects_msg) +
-                  if waypoint in self.waypoint_fiducial: +
-                      self.waypoint_fiducial[waypoint].append(latest_fiducial) +
-                  else: +
-                      self.waypoint_fiducial[waypoint] = [latest_fiducial] +
-                  # Publish to the /spot/nav/reached_waypoint topic +
-                  self.reached_waypoint_pub.publish(waypoint) +
-                  time.sleep(0.5) +
-      def extract_fiducials(self, msg: WorldObjectArray) -> typing.List["Fiducial"]: +
-          self.world_objects: typing.List[WorldObject] = msg.world_objects +
-          fiducial_list = [] +
-          # Iterate through the fiducials in the message, append the x,y,z coordinates to a dictionary +
-          for world_object in self.world_objects: +
-              april_tag: AprilTagProperties = world_object.apriltag_properties +
-              latest_snapshot: FrameTreeSnapshot = world_object.frame_tree_snapshot +
-              # Check if april_tag is None +
-              if april_tag is None: +
-                  continue +
-              # Create the FrameTreeSnapshot as a dictionary +
-              frame_tree_snapshot: typing.Dict[str, PoseStamped] = {} +
-              for child, parent_edge in zip( +
-                  latest_snapshot.child_edges, latest_snapshot.parent_edges +
-              ): +
-                  parent_edge_transform = PoseStamped() +
-                  parent_edge_transform.header.stamp = world_object.acquisition_time +
-                  parent_edge_transform.header.frame_id = parent_edge.parent_frame_name +
-                  parent_edge_transform.pose = parent_edge.parent_tform_child +
-                  frame_tree_snapshot[child] = parent_edge_transform +
-              # Use tf2 to get the april_tag pose in the body frame +
-              april_tag_pose = frame_tree_snapshot[f"fiducial_{april_tag.tag_id}"] +
-              april_tag_pose_filtered = frame_tree_snapshot[ +
-                  f"filtered_fiducial_{april_tag.tag_id}" +
-              ] +
-              # Build the april_tag into the Fiducial class +
-              fiducial = Fiducial( +
-                  tag_id=april_tag.tag_id, +
-                  dim_x=april_tag.x, +
-                  dim_y=april_tag.y, +
-                  fiducial_pose=april_tag_pose, +
-                  filtered_fiducial_pose=april_tag_pose_filtered, +
-                  pose_covariance=april_tag.detection_covariance, +
-                  pose_covariance_frame=april_tag.detection_covariance_reference_frame, +
-              ) +
-              # Append the fiducial to the list +
-              fiducial_list.append(fiducial) +
-          return fiducial_list +
-      def startup(self): +
-          rospy.loginfo("SpotNav robot starting up") +
-          # Call the /spot/claim, /spot/power_on, /spot/stand service +
-          self.call_service("/spot/claim", TriggerRequest()) +
-          self.call_service("/spot/power_on", TriggerRequest()) +
-      def shutdown(self): +
-          rospy.loginfo("SpotNav node shutting down") +
-          # Save the fiducials to a pickle file +
-          with open(f"fiducials_seen.pickle_{time.time()}", "wb") as f: +
-              pickle.dump(self.fiducials_seen, f) +
-          with open(f"waypoint_fiducial.pickle_{time.time()}", "wb") as f: +
-              pickle.dump(self.waypoint_fiducial, f) +
-          # Call the /spot/sit, /spot/power_off, /spot/release service +
-          self.call_service("/spot/sit", TriggerRequest()) +
-          time.sleep(5) +
-          self.call_service("/spot/power_off", TriggerRequest()) +
-          self.call_service("/spot/release", TriggerRequest()) +
-      def main(self): +
-          rospy.init_node("spot_nav", anonymous=True) +
-          self.rates = rospy.get_param("~rates", {}) +
-          if "loop_frequency" in self.rates: +
-              loop_rate = self.rates["loop_frequency"] +
-          else: +
-              loop_rate = 50 +
-          # Initialize the node +
-          rate = rospy.Rate(loop_rate) +
-          rospy.loginfo("SpotNav node started") +
-          self.initialize_subscribers() +
-          self.initialize_publishers() +
-          self.initialize_action_clients() +
-          rospy.on_shutdown(self.shutdown) +
-          # Walk the current graph +
-          self.startup() +
-          self.walk_current_graph() +
-          while not rospy.is_shutdown(): +
-              rate.sleep() +
-  if __name__ == "__main__": +
-      spot_nav = SpotNav() +
-      spot_nav.main() +
-  </code> +
- +
-===== ROS Topics ===== +
- +
-The available ROS topics and thier descriptions are in this section.+
  
 ^ Topic                          ^ Type                ^ Description                                           ^ ^ Topic                          ^ Type                ^ Description                                           ^
Line 356: Line 147:
 | /spot/locked_stop | Trigger | Stop the robot and disallow motion | | /spot/locked_stop | Trigger | Stop the robot and disallow motion |
 | /spot/spot_ros/tf2_frames | Trigger | Get tf2 transformation frames as a FrameGraph | | /spot/spot_ros/tf2_frames | Trigger | Get tf2 transformation frames as a FrameGraph |
 +
 +===== ROS Topics =====
 +
 +^ Topic                                                ^ Type                                       ^ Description                                  ^
 +| /clock                                               | rosgraph_msgs/Clock                        | ROS simulation time                         |
 +| /diagnostics                                         | diagnostic_msgs/DiagnosticArray            | ROS diagnostics                             |
 +| /joint_states                                        | sensor_msgs/JointState                     | Spot joint pose states                      |
 +| /rosout                                               | rosgraph_msgs/Log                          | Logging messages                            |
 +| /rosout_agg                                          | rosgraph_msgs/Log                          | Logging messages                            |
 +| /spot/body_pose/status                               | actionlib_msgs/GoalStatusArray             | Body pose from standing action server       |
 +| /spot/camera/back/camera_info                        | sensor_msgs/CameraInfo                     | Spot monochrome fisheye camera info         |
 +| /spot/camera/back/image                              | sensor_msgs/Image                          | Spot monochrome fisheye camera data         |
 +| /spot/camera/frontleft/camera_info                   | sensor_msgs/CameraInfo                     | Spot monochrome fisheye camera info         |
 +| /spot/camera/frontleft/image                         | sensor_msgs/Image                          | Spot monochrome fisheye camera data         |
 +| /spot/camera/frontright/camera_info                  | sensor_msgs/CameraInfo                     | Spot monochrome fisheye camera info         |
 +| /spot/camera/frontright/image                        | sensor_msgs/Image                          | Spot monochrome fisheye camera data         |
 +| /spot/camera/hand_color/camera_info                 | sensor_msgs/CameraInfo                     | Spot colour hand camera info                |
 +| /spot/camera/hand_color/image                        | sensor_msgs/Image                          | Spot colour hand camera data                |
 +| /spot/camera/hand_mono/camera_info                  | sensor_msgs/CameraInfo                     | Spot monochrome hand camera info            |
 +| /spot/camera/hand_mono/image                         | sensor_msgs/Image                          | Spot monochrome hand camera data            |
 +| /spot/camera/hand/depth_in_color/camera_info        | sensor_msgs/CameraInfo                     | Spot colour and depth hand camera data      |
 +| /spot/camera/left/camera_info                        | sensor_msgs/CameraInfo                     | Spot monochrome fisheye camera info         |
 +| /spot/camera/left/image                              | sensor_msgs/Image                          | Spot monochrome fisheye camera data         |
 +| /spot/camera/right/camera_info                       | sensor_msgs/CameraInfo                     | Spot monochrome fisheye camera info         |
 +| /spot/camera/right/image                             | sensor_msgs/Image                          | Spot monochrome fisheye camera data         |
 +| /spot/depth/back/camera_info                         | sensor_msgs/CameraInfo                     | Spot depth camera info                      |
 +| /spot/depth/back/image                               | sensor_msgs/Image                          | Spot depth camera data                      |
 +| /spot/depth/frontleft/camera_info                    | sensor_msgs/CameraInfo                     | Spot depth camera info                      |
 +| /spot/depth/frontleft/image                          | sensor_msgs/Image                          | Spot depth camera data                      |
 +| /spot/depth/frontright/camera_info                   | sensor_msgs/CameraInfo                     | Spot depth camera info                      |
 +| /spot/depth/frontright/image                         | sensor_msgs/Image                          | Spot depth camera data                      |
 +| /spot/depth/hand/camera_info                         | sensor_msgs/CameraInfo                     | Spot depth hand camera info                 |
 +| /spot/depth/hand/depth_in_color                      | sensor_msgs/Image                          | Spot depth colour hand camera data          |
 +| /spot/depth/hand/image                               | sensor_msgs/Image                          | Spot depth hand camera data                 |
 +| /spot/depth/left/camera_info                         | sensor_msgs/CameraInfo                     | Spot depth in visual camera info |
 +| /spot/depth/left/image | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/depth/right/camera_info | sensor_msgs/CameraInfo | Spot depth in visual camera info |
 +| /spot/depth/right/image | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/depth/back/depth_in_visual/camera_info | sensor_msgs/CameraInfo | Spot depth in visual camera info |
 +| /spot/depth/back/depth_in_visual | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/depth/frontleft/depth_in_visual/camera_info | sensor_msgs/CameraInfo | Spot depth in visual camera info |
 +| /spot/depth/frontleft/depth_in_visual | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/depth/frontright/depth_in_visual/camera_info | sensor_msgs/CameraInfo | Spot depth in visual camera info |
 +| /spot/depth/frontright/depth_in_visual/camera_info | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/depth/left/depth_in_visual/camera_info | sensor_msgs/CameraInfo | Spot depth in visual camera info |
 +| /spot/depth/left/depth_in_visual | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/depth/right/depth_in_visual/camera_info | sensor_msgs/CameraInfo | Spot depth in visual camera info |
 +| /spot/depth/right/depth_in_visual | sensor_msgs/Image | Spot depth in visual camera data |
 +| /spot/dock/status | actionlib_msgs/GoalStatusArray | Spot docking status |
 +| /spot/lidar/points | sensor_msgs/PointCloud2 | Spot EAP Velodye VLP-16 LIDAR data |
 +| /spot/motion_or_idle_body_pose/status | actionlib_msgs/GoalStatusArray | Body pose with movement action server |
 +| /spot/navigate_to/status | actionlib_msgs/GoalStatusArray | GraphNav navigation action server |
 +| /spot/odometry | nav_msgs/Odometry | Spot odometry, "vision" or "odom" |
 +| /spot/odometry/twist | geometry_msgs/TwistWithCovarianceStamped | Legacy compatibility with /spot/cmd_vel |
 +| /spot/status/battery_states | spot_msgs/BatteryStateArray | Spot status: battery state |
 +| /spot/status/behavior_faults | spot_msgs/BehaviorFaultState | Spot status: behavior fault |
 +| /spot/status/estop | spot_msgs/EStopStateArray | Spot status: emergency stop (estop) |
 +| /spot/status/feedback | spot_msgs/Feedback | Spot status: standing, sitting, moving, version |
 +| /spot/status/feet | spot_msgs/FootStateArray | Spot status: feet state |
 +| /spot/status/leases | spot_msgs/LeaseArray | Spot status: active lease holder |
 +| /spot/status/metrics | spot_msgs/Metrics | Spot status: distance and power usage metrics |
 +| /spot/status/mobility_params | spot_msgs/MobilityParams | Spot status: velocity limit, other parameters |
 +| /spot/status/motion_allowed | std_msgs/Bool | Spot status: motion enabled status |
 +| /spot/status/power_state | spot_msgs/PowerState | Spot status: charging, shore power, battery power |
 +| /spot/status/system_faults | spot_msgs/SystemFaultState | Spot status: system fault |
 +| /spot/status/wifi | spot_msgs/WiFiState | Spot status: wifi ESSID, current mode |
 +| /spot/trajectory/status | actionlib_msgs/GoalStatusArray | Trajectory pose movement action server |
 +| /spot/world_objects | spot_msgs/WorldObjectArray | Spot world object detections |
 +| /tf | tf2_msgs/TFMessage | Transformation frames from TF2 API |
 +| /tf_static | tf2_msgs/TFMessage | Transformation frames from TF2 API |
 +| /twist_marker_server/update | visualization_msgs/InteractiveMarkerUpdate | Legacy compatibility with /spot/cmd_vel |
 +| /twist_marker_server/update_full | visualization_msgs/InteractiveMarkerInit | Legacy compatibility with /spot/cmd_vel |
 +
 +===== ROS Actions =====
 +
 +^ Topic ^ Type ^ Description ^
 +| /spot/navigate_to | NavigateToAction | Go to a GraphNav waypoint |
 +| /spot/navigate_route | NavigateRouteAction | Go to a series of GraphNav waypoints |
 +| /spot/trajectory | TrajectoryAction | Move robot to a Pose |
 +| /spot/motion_or_idle_body_pose | PoseBodyAction | Rotate body to a pose |
 +| /spot/body_pose | PoseBodyAction | Stand and move robot to a Pose |
 +| /spot/dock | DockAction | Dock or undock at a specific dock_id |
  
 ===== Frequently Asked Questions (FAQ) ===== ===== Frequently Asked Questions (FAQ) =====
Line 372: Line 245:
  
 [[https://github.com/heuristicus/spot_ros|spot_ros Github]] [[https://github.com/heuristicus/spot_ros|spot_ros Github]]
 +
 [[https://github.com/bdaiinstitute/spot_wrapper|spot_wrapper Github]] [[https://github.com/bdaiinstitute/spot_wrapper|spot_wrapper Github]]
 +
 [[https://github.com/bdaiinstitute/spot_ros2|spot_ros2 Github]] [[https://github.com/bdaiinstitute/spot_ros2|spot_ros2 Github]]
 +
 +A snapshot of these repos are also available on the internal gitlab:
 +
 +https://git.cs.lth.se/robotlab/spot
  • documentation/robots/spot/spot_ros.1682364239.txt.gz
  • Last modified: 2023/04/24 19:23
  • by mings