Squashed commit of the following:

commit 0424b961e75b0e8f4143424e9fb0121ee5b3c01c Author: Dave Niewinski <dniewinski@clearpathrobotics.com> Date: Mon May 11 16:36:55 2020 -0400 Updated logging commit afdc5301f2b73f219b51ae3ce7c56e0f036e75a6 Author: Dave Niewinski <dniewinski@clearpathrobotics.com> Date: Mon May 11 15:00:27 2020 -0400 Added launch and params commit 8c1066108d3cc2955cf49a73a75e3d249a8704d2 Author: Dave Niewinski <dniewinski@clearpathrobotics.com> Date: Fri May 8 15:04:48 2020 -0400 Basic ros support implemented. Outputs joint angles, and odom commit 3f71252b182738234cc54e581cac3b8a54874733 Author: Dave Niewinski <dniewinski@clearpathrobotics.com> Date: Wed May 6 16:46:21 2020 -0400 Basic functionality, just printing in the terminal. commit 505c17e1d4d5a28d14872d81e2f11b60b61135e9 Author: Dave Niewinski <dniewinski@clearpathrobotics.com> Date: Wed May 6 14:00:42 2020 -0400 Initial pass at data from robot
csl-taipeitech · May 11, 2020 · 3754770 · 3754770
1 parent 50b6c68
commit 3754770
Show file tree

Hide file tree

Showing 7 changed files with 392 additions and 0 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1 @@
+*.pyc
diff --git a/spot_driver/CMakeLists.txt b/spot_driver/CMakeLists.txt
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spot_driver)
+
+find_package(catkin REQUIRED COMPONENTS)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
diff --git a/spot_driver/config/spot_ros.yaml b/spot_driver/config/spot_ros.yaml
@@ -0,0 +1,8 @@
+rates:
+  robot_state: 20.0
+  metrics: 1.0
+  robot_command: 1.0
+  power: 1.0
+  lease: 1.0
+  image: 1.0
+  estop: 1.0
diff --git a/spot_driver/launch/driver.launch b/spot_driver/launch/driver.launch
@@ -0,0 +1,17 @@
+<launch>
+  <arg name="username" default="dummyusername" />
+  <arg name="password" default="dummypassword" />
+  <arg name="app_token" default="/home/administrator/.bosdyn/dev.app_token" />
+  <arg name="hostname" default="192.168.131.3" />
+
+  <include file="$(find spot_description)/launch/description.launch" />
+
+  <node pkg="spot_driver" type="spot_ros.py" name="spot_ros" >
+    <rosparam file="$(find spot_driver)/config/spot_ros.yaml" command="load" />
+    <param name="username" value="$(arg username)" />
+    <param name="password" value="$(arg password)" />
+    <param name="app_token" value="$(arg app_token)" />
+    <param name="hostname" value="$(arg hostname)" />
+  </node>
+
+</launch>
diff --git a/spot_driver/package.xml b/spot_driver/package.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>spot_driver</name>
+  <version>0.0.0</version>
+  <description>The spot_driver package</description>
+
+  <maintainer email="dniewinski@clearpathrobotics.com">Dave Niewinski</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+</package>
diff --git a/spot_driver/scripts/spot_ros.py b/spot_driver/scripts/spot_ros.py
@@ -0,0 +1,138 @@
+#!/usr/bin/env python3
+import rospy
+
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import JointState
+
+from spot_wrapper import SpotWrapper
+import logging
+
+SW = None
+
+back_fisheye_image_pub = rospy.Publisher('back_fisheye_image', Image, queue_size=10)
+frontleft_fisheye_image_pub = rospy.Publisher('frontleft_fisheye_image', Image, queue_size=10)
+frontright_fisheye_image_pub = rospy.Publisher('frontright_fisheye_image', Image, queue_size=10)
+left_fisheye_image_pub = rospy.Publisher('left_fisheye_image', Image, queue_size=10)
+right_fisheye_image_pub = rospy.Publisher('right_fisheye_image', Image, queue_size=10)
+
+joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+tf_pub = rospy.Publisher('tf', TFMessage, queue_size=10)
+
+friendly_joint_names = {}
+friendly_joint_names["fl.hx"] = "front_left_hip_x"
+friendly_joint_names["fl.hy"] = "front_left_hip_y"
+friendly_joint_names["fl.kn"] = "front_left_knee"
+friendly_joint_names["fr.hx"] = "front_right_hip_x"
+friendly_joint_names["fr.hy"] = "front_right_hip_y"
+friendly_joint_names["fr.kn"] = "front_right_knee"
+friendly_joint_names["hl.hx"] = "rear_left_hip_x"
+friendly_joint_names["hl.hy"] = "rear_left_hip_y"
+friendly_joint_names["hl.kn"] = "rear_left_knee"
+friendly_joint_names["hr.hx"] = "rear_right_hip_x"
+friendly_joint_names["hr.hy"] = "rear_right_hip_y"
+friendly_joint_names["hr.kn"] = "rear_right_knee"
+
+def RobotStateCB(results):
+    state = SW.robot_state
+
+    if state:
+        joint_state = JointState()
+        joint_state.header.stamp = rospy.Time.now()
+        for joint in state.kinematic_state.joint_states:
+            joint_state.name.append(friendly_joint_names.get(joint.name, "ERROR"))
+            joint_state.position.append(joint.position.value)
+            joint_state.velocity.append(joint.velocity.value)
+            joint_state.effort.append(joint.load.value)
+
+        joint_state_pub.publish(joint_state)
+
+        tf_msg = TFMessage()
+        for frame_name in state.kinematic_state.transforms_snapshot.child_to_parent_edge_map:
+            if state.kinematic_state.transforms_snapshot.child_to_parent_edge_map.get(frame_name).parent_frame_name:
+                transform = state.kinematic_state.transforms_snapshot.child_to_parent_edge_map.get(frame_name)
+                new_tf = TransformStamped()
+                new_tf.header.stamp = rospy.Time.now()
+                new_tf.header.frame_id = transform.parent_frame_name
+                new_tf.child_frame_id = frame_name
+                new_tf.transform.translation.x = transform.parent_tform_child.position.x
+                new_tf.transform.translation.y = transform.parent_tform_child.position.y
+                new_tf.transform.translation.z = transform.parent_tform_child.position.z
+                new_tf.transform.rotation.x = transform.parent_tform_child.rotation.x
+                new_tf.transform.rotation.y = transform.parent_tform_child.rotation.y
+                new_tf.transform.rotation.z = transform.parent_tform_child.rotation.z
+                new_tf.transform.rotation.w = transform.parent_tform_child.rotation.w
+                tf_msg.transforms.append(new_tf)
+        if len(tf_msg.transforms) > 0:
+            tf_pub.publish(tf_msg)
+
+def MetricsCB(results):
+    rospy.logdebug("##### METRICS #####")
+    #rospy.loginfo(str(SW.metrics))
+
+def RobotCommandCB(results):
+    rospy.logdebug("##### COMMAND #####")
+    #rospy.loginfo(str(SW.robot_command))
+
+def PowerCB(results):
+    rospy.logdebug("##### POWER #####")
+    #rospy.loginfo(str(SW.power))
+
+def LeaseCB(results):
+    rospy.logdebug("##### LEASE #####")
+    #rospy.loginfo(str(SW.lease))
+
+def getImageMsg(data):
+    msg = Image()
+    #Header header
+    msg.height = data.shot.image.rows
+    msg.width = data.shot.image.cols
+    #string encoding
+    #uint8 is_bigendian
+    #uint32 step
+    msg.data = data.shot.image.data
+
+def ImageCB(results):
+    data = SW.image
+    #if data:
+    #    back_fisheye_image_pub.publish(getImageMsg(data[0]))
+    #    frontleft_fisheye_image.publish(getImageMsg(data[1]))
+    #    frontright_fisheye_image.publish(getImageMsg(data[2]))
+    #    left_fisheye_image.publish(getImageMsg(data[3]))
+    #    right_fisheye_image.publish(getImageMsg(data[4]))
+
+def EstopCB(results):
+    rospy.logdebug("##### ESTOP #####")
+
+callbacks = {}
+callbacks["robot_state"] = RobotStateCB
+callbacks["metrics"] = MetricsCB
+callbacks["robot_command"] = RobotCommandCB
+callbacks["power"] = PowerCB
+callbacks["lease"] = LeaseCB
+callbacks["image"] = ImageCB
+callbacks["estop"] = EstopCB
+
+rospy.init_node('spot_ros', anonymous=True)
+rate = rospy.Rate(50)
+
+rates = rospy.get_param('~rates', {})
+username = rospy.get_param('~username', 'default_value')
+password = rospy.get_param('~password', 'default_value')
+app_token = rospy.get_param('~app_token', 'default_value')
+hostname = rospy.get_param('~hostname', 'default_value')
+
+logger = logging.getLogger('rosout')
+
+rospy.loginfo("Starting")
+SW = SpotWrapper(username, password, app_token, hostname, logger, rates, callbacks)
+
+if SW._robot:
+    rospy.loginfo("Connecting")
+    SW.connect()
+    rospy.loginfo("Running")
+    with SW.getLease():
+        while not rospy.is_shutdown():
+            SW.updateTasks()
+            rate.sleep()
diff --git a/spot_driver/scripts/spot_wrapper.py b/spot_driver/scripts/spot_wrapper.py
@@ -0,0 +1,201 @@
+import time
+
+from bosdyn.client import create_standard_sdk, ResponseError, RpcError
+from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks
+
+from bosdyn.client.spot_check import SpotCheckClient
+from bosdyn.client.robot_state import RobotStateClient
+from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder
+from bosdyn.client.power import PowerClient
+from bosdyn.client.lease import LeaseClient, LeaseKeepAlive
+from bosdyn.client.image import ImageClient
+from bosdyn.client.estop import EstopClient, EstopEndpoint, EstopKeepAlive
+
+import bosdyn.api.robot_state_pb2 as robot_state_proto
+
+image_sources = ['back_fisheye_image', 'frontleft_fisheye_image', 'frontright_fisheye_image', 'left_fisheye_image', 'right_fisheye_image']
+
+class AsyncRobotState(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncRobotState, self).__init__("robot-state", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.get_robot_state_async()
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class AsyncMetrics(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncMetrics, self).__init__("robot-metrics", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.get_robot_metrics_async()
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class AsyncRobotCommand(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncRobotCommand, self).__init__("robot-command", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.robot_command_feedback_async()
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class AsyncPower(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncPower, self).__init__("power", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.power_command_feedback_async()
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class AsyncLease(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncLease, self).__init__("lease", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.list_leases_async()
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class AsyncImageService(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncImageService, self).__init__("robot_image_service", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.get_image_from_sources_async(image_sources)
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class AsyncEStop(AsyncPeriodicQuery):
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncEStop, self).__init__("estop", client, logger,
+                                           period_sec=1.0/rate)
+        self._callback = callback
+
+    def _start_query(self):
+        callback_future = self._client.get_status_async()
+        callback_future.add_done_callback(self._callback)
+        return callback_future
+
+class SpotWrapper():
+    def __init__(self, username, password, token, hostname, logger, rates = {}, callbacks = {}):
+        self._username = username
+        self._password = password
+        self._token = token
+        self._hostname = hostname
+        self._logger = logger
+        self._rates = rates
+        self._callbacks = callbacks
+        self._keep_alive = True
+
+        self._sdk = create_standard_sdk('ros_spot')
+        self._sdk.load_app_token(self._token)
+        self._robot = self._sdk.create_robot(self._hostname)
+
+        try:
+            self._robot.authenticate(self._username, self._password)
+            self._robot.start_time_sync()
+        except RpcError as err:
+            self._logger.error("Failed to communicate with robot: %s", err)
+            self._robot = None
+
+        if self._robot:
+            # Clients
+            self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name)
+            #self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name)
+            #self._power_client = self._robot.ensure_client(PowerClient.default_service_name)
+            self._lease_client = self._robot.ensure_client(LeaseClient.default_service_name)
+            self._image_client = self._robot.ensure_client(ImageClient.default_service_name)
+            self._estop_client = self._robot.ensure_client(EstopClient.default_service_name)
+
+            # Async Tasks
+            self._robot_state_task = AsyncRobotState(self._robot_state_client, self._logger, self._rates.get("robot_state", 1.0), self._callbacks.get("robot_state", lambda:None))
+            self._robot_metrics_task = AsyncMetrics(self._robot_state_client, self._logger, self._rates.get("metrics", 1.0), self._callbacks.get("metrics", lambda:None))
+            #self._robot_command_task = AsyncRobotCommand(self._robot_command_client, self._logger, self._rates.get("robot_command", 1.0), self._callbacks.get("robot_command", lambda:None))
+            #self._power_task = AsyncPower(self._power_client, self._logger, self._rates.get("power", 1.0), self._callbacks.get("power", lambda:None))
+            self._lease_task = AsyncLease(self._lease_client, self._logger, self._rates.get("lease", 1.0), self._callbacks.get("lease", lambda:None))
+            self._image_task = AsyncImageService(self._image_client, self._logger, self._rates.get("image", 1.0), self._callbacks.get("image", lambda:None))
+            self._estop_task = AsyncEStop(self._estop_client, self._logger, self._rates.get("estop", 1.0), self._callbacks.get("estop", lambda:None))
+
+            self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0)
+
+            self._async_tasks = AsyncTasks(
+                [self._robot_state_task, self._robot_metrics_task, self._lease_task, self._image_task, self._estop_task])
+                #[self._robot_state_task, self._robot_metrics_task, self._robot_command_task, self._power_task, self._lease_task, self._image_task, self._estop_task])
+
+            self._robot_id = None
+            self._lease = None
+
+    @property
+    def spot_check(self):
+        return self._spot_check_task.proto
+
+    @property
+    def robot_state(self):
+        return self._robot_state_task.proto
+
+    @property
+    def metrics(self):
+        return self._robot_metrics_task.proto
+
+    @property
+    def robot_command(self):
+        return self._robot_command_task.proto
+
+    @property
+    def power(self):
+        return self._power_task.proto
+
+    @property
+    def lease(self):
+        return self._lease_task.proto
+
+    @property
+    def image(self):
+        return self._image_task.proto
+
+    @property
+    def estop(self):
+        return self._estop_task.proto
+
+    def connect(self):
+        try:
+            self._robot_id = self._robot.get_id()
+            self._lease = self._lease_client.acquire()
+            self._estop_endpoint.force_simple_setup()  # Set this endpoint as the robot's sole estop.
+        except (ResponseError, RpcError) as err:
+            self._logger.error("Failed to initialize robot communication: %s", err)
+            return False
+
+    def updateTasks(self):
+        self._async_tasks.update()
+
+    def getLease(self):
+        return LeaseKeepAlive(self._lease_client)
+
+    def disconnect(self):
+        """Release control of robot as gracefully as posssible."""
+        self._logger.info("Shutting down ROS interface")
+        if self._robot.time_sync:
+            self._robot.time_sync.stop()
+        if self._estop_keepalive:
+            self._estop_keepalive.stop()
+            self._estop_keepalive = None
+        if self._lease:
+            self._lease_client.return_lease(self._lease)
+            self._lease = None