Added services for power, stand, sit, self-right, and stop

spot_driver/config/spot_ros.yaml

@@ -1,10 +1,7 @@
 rates:
   robot_state: 20.0
   metrics: 0.2
-  robot_command: 1.0
-  power: 1.0
   lease: 1.0
   front_image: 10.0
   side_image: 10.0
   rear_image: 10.0
-  estop: 1.0

spot_driver/scripts/ros_helpers.py

@@ -1,11 +1,22 @@
 import rospy
 
+from std_msgs.msg import Empty
 from tf2_msgs.msg import TFMessage
 from geometry_msgs.msg import TransformStamped
 from sensor_msgs.msg import Image, CameraInfo
 from sensor_msgs.msg import JointState
 from geometry_msgs.msg import TwistWithCovarianceStamped
 
+from spot_msgs.msg import Metrics
+from spot_msgs.msg import LeaseArray, LeaseResource
+from spot_msgs.msg import FootState, FootStateArray
+from spot_msgs.msg import EStopState, EStopStateArray
+from spot_msgs.msg import WiFiState
+from spot_msgs.msg import PowerState
+from spot_msgs.msg import BehaviorFault, BehaviorFaultState
+from spot_msgs.msg import SystemFault, SystemFaultState
+from spot_msgs.msg import BatteryState, BatteryStateArray
+
 friendly_joint_names = {}
 """Dictionary for mapping BD joint names to more friendly names"""
 friendly_joint_names["fl.hx"] = "front_left_hip_x"
@@ -244,48 +255,48 @@ def GetPowerStatesFromState(state):
     power_state_msg.locomotion_estimated_runtime = rospy.Time(state.power_state.locomotion_estimated_runtime.seconds, state.power_state.locomotion_estimated_runtime.nanos)
     return power_state_msg
 
-    def getBehaviorFaults(behavior_faults):
-        """Helper function to strip out behavior faults into a list
+def getBehaviorFaults(behavior_faults):
+    """Helper function to strip out behavior faults into a list
 
-        Args:
-            behavior_faults: List of BehaviorFaults
-        """
-        faults = []
+    Args:
+        behavior_faults: List of BehaviorFaults
+    """
+    faults = []
 
-        for fault in behavior_faults:
-            new_fault = BehaviorFault()
-            new_fault.behavior_fault_id = fault.behavior_fault_id
-            new_fault.header.stamp = rospy.Time(fault.onset_timestamp.seconds, fault.onset_timestamp.nanos)
-            new_fault.cause = fault.cause
-            new_fault.status = fault.status
-            faults.append(new_fault)
+    for fault in behavior_faults:
+        new_fault = BehaviorFault()
+        new_fault.behavior_fault_id = fault.behavior_fault_id
+        new_fault.header.stamp = rospy.Time(fault.onset_timestamp.seconds, fault.onset_timestamp.nanos)
+        new_fault.cause = fault.cause
+        new_fault.status = fault.status
+        faults.append(new_fault)
 
-        return faults
+    return faults
 
-    def getSystemFaults(system_faults):
-        """Helper function to strip out system faults into a list
+def getSystemFaults(system_faults):
+    """Helper function to strip out system faults into a list
 
-        Args:
-            systen_faults: List of SystemFaults
-        """
-        faults = []
+    Args:
+        systen_faults: List of SystemFaults
+    """
+    faults = []
 
-        for fault in system_faults:
-            new_fault = SystemFault()
-            new_fault.name = fault.name
-            new_fault.header.stamp = rospy.Time(fault.onset_timestamp.seconds, fault.onset_timestamp.nanos)
-            new_fault.duration = rospy.Time(fault.duration.seconds, fault.duration.nanos)
-            new_fault.code = fault.code
-            new_fault.uid = fault.uid
-            new_fault.error_message = fault.error_message
+    for fault in system_faults:
+        new_fault = SystemFault()
+        new_fault.name = fault.name
+        new_fault.header.stamp = rospy.Time(fault.onset_timestamp.seconds, fault.onset_timestamp.nanos)
+        new_fault.duration = rospy.Time(fault.duration.seconds, fault.duration.nanos)
+        new_fault.code = fault.code
+        new_fault.uid = fault.uid
+        new_fault.error_message = fault.error_message
 
-            for att in fault.attributes:
-                new_fault.attributes.append(att)
+        for att in fault.attributes:
+            new_fault.attributes.append(att)
 
-            new_fault.severity = fault.severity
-            faults.append(new_fault)
+        new_fault.severity = fault.severity
+        faults.append(new_fault)
 
-        return faults
+    return faults
 
 def GetSystemFaultsFromState(state):
     system_fault_state_msg = SystemFaultState()

spot_driver/scripts/spot_ros.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import rospy
 
+from std_srvs.srv import Trigger, TriggerResponse
 from tf2_msgs.msg import TFMessage
 from geometry_msgs.msg import TransformStamped
 from sensor_msgs.msg import Image, CameraInfo
@@ -32,13 +33,10 @@ def __init__(self):
         """Dictionary listing what callback to use for what data task"""
         self.callbacks["robot_state"] = self.RobotStateCB
         self.callbacks["metrics"] = self.MetricsCB
-        self.callbacks["robot_command"] = self.RobotCommandCB
-        self.callbacks["power"] = self.PowerCB
         self.callbacks["lease"] = self.LeaseCB
         self.callbacks["front_image"] = self.FrontImageCB
         self.callbacks["side_image"] = self.SideImageCB
         self.callbacks["rear_image"] = self.RearImageCB
-        self.callbacks["estop"] = self.EstopCB
 
     def RobotStateCB(self, results):
         """Callback for when the Spot Wrapper gets new robot state data.
@@ -113,26 +111,6 @@ def MetricsCB(self, results):
 
             self.metrics_pub.publish(metrics_msg)
 
-    def RobotCommandCB(self, results):
-        """Callback for when the Spot Wrapper gets new robot command data.
-
-        Args:
-            results: FutureWrapper object of AsyncPeriodicQuery callback
-        """
-        # TODO: All of this
-        rospy.logdebug("##### COMMAND #####")
-        #rospy.loginfo(str(self.spot_wrapper.robot_command))
-
-    def PowerCB(self, results):
-        """Callback for when the Spot Wrapper gets new power data.
-
-        Args:
-            results: FutureWrapper object of AsyncPeriodicQuery callback
-        """
-        # TODO: All of this
-        rospy.logdebug("##### POWER #####")
-        #rospy.logwarn(str(self.spot_wrapper.power))
-
     def LeaseCB(self, results):
         """Callback for when the Spot Wrapper gets new lease data.
 
@@ -169,19 +147,19 @@ def FrontImageCB(self, results):
             image_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(data[0])
             self.frontleft_image_pub.publish(image_msg0)
             self.frontleft_image_info_pub.publish(camera_info_msg0)
-            self.tf_pub.publish(tf_msg0)
+            self.tf_pub.publish(camera_tf_msg0)
             image_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(data[1])
             self.frontright_image_pub.publish(image_msg1)
             self.frontright_image_info_pub.publish(camera_info_msg1)
-            self.tf_pub.publish(tf_msg1)
+            self.tf_pub.publish(camera_tf_msg1)
             image_msg2, camera_info_msg2, camera_tf_msg2 = getImageMsg(data[2])
             self.frontleft_depth_pub.publish(image_msg2)
             self.frontleft_depth_info_pub.publish(camera_info_msg2)
-            self.tf_pub.publish(tf_msg2)
+            self.tf_pub.publish(camera_tf_msg2)
             image_msg3, camera_info_msg3, camera_tf_msg3 = getImageMsg(data[3])
             self.frontright_depth_pub.publish(image_msg3)
             self.frontright_depth_info_pub.publish(camera_info_msg3)
-            self.tf_pub.publish(tf_msg3)
+            self.tf_pub.publish(camera_tf_msg3)
 
     def SideImageCB(self, results):
         """Callback for when the Spot Wrapper gets new side image data.
@@ -194,19 +172,19 @@ def SideImageCB(self, results):
             image_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(data[0])
             self.left_image_pub.publish(image_msg0)
             self.left_image_info_pub.publish(camera_info_msg0)
-            self.tf_pub.publish(tf_msg0)
+            self.tf_pub.publish(camera_tf_msg0)
             image_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(data[1])
             self.right_image_pub.publish(image_msg1)
             self.right_image_info_pub.publish(camera_info_msg1)
-            self.tf_pub.publish(tf_msg1)
+            self.tf_pub.publish(camera_tf_msg1)
             image_msg2, camera_info_msg2, camera_tf_msg2 = getImageMsg(data[2])
             self.left_depth_pub.publish(image_msg2)
             self.left_depth_info_pub.publish(camera_info_msg2)
-            self.tf_pub.publish(tf_msg2)
+            self.tf_pub.publish(camera_tf_msg2)
             image_msg3, camera_info_msg3, camera_tf_msg3 = getImageMsg(data[3])
             self.right_depth_pub.publish(image_msg3)
             self.right_depth_info_pub.publish(camera_info_msg3)
-            self.tf_pub.publish(tf_msg3)
+            self.tf_pub.publish(camera_tf_msg3)
 
     def RearImageCB(self, results):
         """Callback for when the Spot Wrapper gets new rear image data.
@@ -219,19 +197,35 @@ def RearImageCB(self, results):
             mage_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(data[0])
             self.back_image_pub.publish(mage_msg0)
             self.back_image_info_pub.publish(camera_info_msg0)
-            self.tf_pub.publish(tf_msg0)
+            self.tf_pub.publish(camera_tf_msg0)
             mage_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(data[1])
             self.back_depth_pub.publish(mage_msg1)
             self.back_depth_info_pub.publish(camera_info_msg1)
-            self.tf_pub.publish(tf_msg1)
+            self.tf_pub.publish(camera_tf_msg1)
 
-    def EstopCB(self, results):
-        """Callback for when the Spot Wrapper gets new estop data.
+    def handle_stop(self, req):
+        resp = self.spot_wrapper.stop()
+        return TriggerResponse(resp[0], resp[1])
 
-        Args:
-            results: FutureWrapper object of AsyncPeriodicQuery callback
-        """
-        rospy.logdebug("##### ESTOP #####")
+    def handle_self_right(self, req):
+        resp = self.spot_wrapper.self_right()
+        return TriggerResponse(resp[0], resp[1])
+
+    def handle_sit(self, req):
+        resp = self.spot_wrapper.sit()
+        return TriggerResponse(resp[0], resp[1])
+
+    def handle_stand(self, req):
+        resp = self.spot_wrapper.stand()
+        return TriggerResponse(resp[0], resp[1])
+
+    def handle_power_on(self, req):
+        resp = self.spot_wrapper.power_on()
+        return TriggerResponse(resp[0], resp[1])
+
+    def handle_safe_power_off(self, req):
+        resp = self.spot_wrapper.safe_power_off()
+        return TriggerResponse(resp[0], resp[1])
 
     def main(self):
         """Main function for the SpotROS class.  Gets config from ROS and initializes the wrapper.  Holds lease from wrapper and updates all async tasks at the ROS rate"""
@@ -276,6 +270,7 @@ def main(self):
             self.left_depth_info_pub = rospy.Publisher('depth/left/camera_info', CameraInfo, queue_size=10)
             self.right_depth_info_pub = rospy.Publisher('depth/right/camera_info', CameraInfo, queue_size=10)
 
+            # Status Publishers #
             self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
             """Defining a TF publisher manually because of conflicts between Python3 and tf"""
             self.tf_pub = rospy.Publisher('tf', TFMessage, queue_size=10)
@@ -290,6 +285,13 @@ def main(self):
             self.behavior_faults_pub = rospy.Publisher('status/behavior_faults', BehaviorFaultState, queue_size=10)
             self.system_faults_pub = rospy.Publisher('status/system_faults', SystemFaultState, queue_size=10)
 
+            rospy.Service("stop", Trigger, self.handle_stop)
+            rospy.Service("self_right", Trigger, self.handle_self_right)
+            rospy.Service("sit", Trigger, self.handle_sit)
+            rospy.Service("stand", Trigger, self.handle_stand)
+            rospy.Service("power_on", Trigger, self.handle_power_on)
+            rospy.Service("power_off", Trigger, self.handle_safe_power_off)
+
             rospy.loginfo("Connecting")
             self.spot_wrapper.connect()
             rospy.loginfo("Running")