Basic motion. Lots to clean up

spot_driver/config/spot_ros.yaml

@@ -5,3 +5,5 @@ rates:
   front_image: 10.0
   side_image: 10.0
   rear_image: 10.0
+auto_power_on: True
+auto_stand: True

spot_driver/config/twist_mux.yaml

@@ -0,0 +1,10 @@
+topics:
+- name    : bt_joy
+  topic   : bluetooth_teleop/cmd_vel
+  timeout : 0.5
+  priority: 9
+- name    : interactive_marker
+  topic   : twist_marker_server/cmd_vel
+  timeout : 0.5
+  priority: 8
+locks: []

spot_driver/launch/driver.launch

@@ -16,4 +16,9 @@
     <remap from="tf" to="/tf"/>
   </node>
 
+  <node pkg="twist_mux" type="twist_mux" name="twist_mux" >
+    <rosparam command="load" file="$(find spot_driver)/config/twist_mux.yaml" />
+    <remap from="cmd_vel_out" to="spot/cmd_vel"/>
+  </node>
+
 </launch>

spot_driver/scripts/ros_helpers.py

@@ -258,6 +258,7 @@ def GetTFFromState(state):
         TFMessage message
     """
     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)

spot_driver/scripts/spot_ros.py

@@ -6,7 +6,10 @@
 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 geometry_msgs.msg import TwistWithCovarianceStamped, Twist, Pose
+
+from bosdyn.api.geometry_pb2 import Quaternion
+import bosdyn.geometry
 
 from spot_msgs.msg import Metrics
 from spot_msgs.msg import LeaseArray, LeaseResource
@@ -233,6 +236,21 @@ def handle_safe_power_off(self, req):
         resp = self.spot_wrapper.safe_power_off()
         return TriggerResponse(resp[0], resp[1])
 
+    def cmdVelCallback(self, data):
+        """Callback for cmd_vel command"""
+        self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y, data.angular.z, self.mobility_params)
+
+    def bodyPoseCallback(self, data):
+        """Callback for cmd_vel command"""
+        q = Quaternion()
+        q.x = data.orientation.x
+        q.y = data.orientation.y
+        q.z = data.orientation.z
+        q.w = data.orientation.w
+
+        euler_zxy = q.to_euler_zxy()
+        self.mobility_params = self.spot_wrapper.get_mobility_params(data.position.z, euler_zxy)
+
     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"""
         rospy.init_node('spot_ros', anonymous=True)
@@ -248,7 +266,7 @@ def main(self):
 
         rospy.loginfo("Starting")
         self.spot_wrapper = SpotWrapper(self.username, self.password, self.app_token, self.hostname, self.logger, self.rates, self.callbacks)
-
+        self.mobility_params = self.spot_wrapper.get_mobility_params()
         if self.spot_wrapper._robot:
             # Images #
             self.back_image_pub = rospy.Publisher('camera/back/image', Image, queue_size=10)
@@ -291,6 +309,9 @@ 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.Subscriber('cmd_vel', Twist, self.cmdVelCallback)
+            rospy.Subscriber('body_pose', Pose, self.bodyPoseCallback)
+
             rospy.Service("stop", Trigger, self.handle_stop)
             rospy.Service("self_right", Trigger, self.handle_self_right)
             rospy.Service("sit", Trigger, self.handle_sit)
@@ -301,7 +322,16 @@ def main(self):
             rospy.loginfo("Connecting")
             self.spot_wrapper.connect()
             rospy.loginfo("Running")
+
             with self.spot_wrapper.getLease():
+                self.auto_power_on = rospy.get_param('~auto_power_on', False)
+                self.auto_stand = rospy.get_param('~auto_stand', False)
+
+                if self.auto_power_on:
+                    self.spot_wrapper.power_on()
+                    if self.auto_stand:
+                        self.spot_wrapper.stand()
+
                 while not rospy.is_shutdown():
                     self.spot_wrapper.updateTasks()
                     rate.sleep()

spot_driver/scripts/spot_wrapper.py

@@ -2,6 +2,7 @@
 
 from bosdyn.client import create_standard_sdk, ResponseError, RpcError
 from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks
+from bosdyn.geometry import EulerZXY
 
 from bosdyn.client.robot_state import RobotStateClient
 from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder
@@ -326,31 +327,28 @@ def power_on(self):
         except:
             return False, "Error"
 
-    def set_mobility_params(self, body_height, body_yaw, body_roll, body_pitch, locomotion_hint, stair_hint, external_force_params=None):
+    def get_mobility_params(self, body_height=0, footprint_R_body=EulerZXY(), locomotion_hint=1, stair_hint=False, external_force_params=None):
         """Define body, locomotion, and stair parameters.
 
         Args:
             body_height: Body height in meters
-            body_yaw: The yaw of the body frame with respect to the footprint frame in radians
-            body_roll: The roll of the body frame with respect to the footprint frame in radians
-            body_pitch: The pitch of the body frame with respect to the footprint frame in radians
+            footprint_R_body: (EulerZXY) – The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet)
             locomotion_hint: Locomotion hint
             stair_hint: Boolean to define stair motion
         """
-        pass
-        #bosdyn.geometry.EulerZXY object
-        #return self._start_robot_command('mobility-params', RobotCommandBuilder.mobility_params(body_heigh, footprint_R_body, locomotion_hint, stair_hint, external_force_params))
+        return RobotCommandBuilder.mobility_params(body_height, footprint_R_body, locomotion_hint, stair_hint, external_force_params)
 
-    def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125):
+    def velocity_cmd(self, v_x, v_y, v_rot, mobility_params, cmd_duration=0.125):
         """Send a velocity motion command to the robot.
 
         Args:
             v_x: Velocity in the X direction in meters
             v_y: Velocity in the Y direction in meters
             v_rot: Angular velocity around the Z axis in radians
-            cmd_duration: (optional) Time-to-live for the command in seconds.  Default is 125ms.
+            mobility_params: mobility parameters
+            cmd_duration: (optional) Time-to-live for the command in seconds.  Default is 125ms (assuming 10Hz command rate).
         """
-        return self._start_robot_command(desc,
+        return self._async_robot_command("ros_cmd_vel",
                                   RobotCommandBuilder.velocity_command(
-                                      v_x=v_x, v_y=v_y, v_rot=v_rot),
+                                      v_x=v_x, v_y=v_y, v_rot=v_rot, params=mobility_params),
                                   end_time_secs=time.time() + cmd_duration)

spot_viz/launch/view_robot.launch

@@ -1,3 +1,8 @@
 <launch>
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find spot_viz)/rviz/model.rviz" />
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find spot_viz)/rviz/robot.rviz" />
+
+  <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" >
+    <param name="link_name" value="body" />
+    <param name="robot_name" value="spot" />
+  </node>
 </launch>

spot_viz/rviz/robot.rviz

@@ -26,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: DepthCloud
+    SyncSource: ""
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -430,6 +430,29 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: DepthClouds
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /twist_marker_server/update
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -455,7 +478,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 5.90245867
+      Distance: 2.3290627
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -470,26 +493,26 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.520398974
+      Pitch: 0.485399008
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.35733771
+      Yaw: 2.21233201
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 1056
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 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
+  Hide Right Dock: true
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
+    collapsed: true
   Width: 1855
   X: 65
   Y: 24