tested body velocity commands

Author: zkytony

spot/ros_ws/src/rbd_spot/src/rbd_spot/__init__.py

@@ -1,3 +1,3 @@
 from rbd_spot_perception import image, graphnav, robot_state, fiducial
 from rbd_spot_robot import SpotSDKConn, state
-from rbd_spot_action import arm
+from rbd_spot_action import arm, body

spot/ros_ws/src/rbd_spot_action/src/rbd_spot_action/body.py

@@ -11,7 +11,7 @@ def create_client(conn):
     return conn.ensure_client(RobotCommandClient.default_service_name)
 
 def velocityCommand(conn, command_client, v_x, v_y, v_rot,
-                    cmd_duration=0.125, mobility_params=None):
+                    duration=0.125, mobility_params=None):
     """
     Send a velocity motion command to the robot.
 
@@ -25,7 +25,7 @@ def velocityCommand(conn, command_client, v_x, v_y, v_rot,
     """
     if mobility_params is None:
         mobility_params = RobotCommandBuilder.mobility_params()
-    end_time = time.time() + cmd_duration
+    end_time = time.time() + duration
     synchro_velocity_command = RobotCommandBuilder.synchro_velocity_command(
         v_x=v_x, v_y=v_y, v_rot=v_rot, params=mobility_params)
 

spot/ros_ws/src/rbd_spot_action/tests/test_velocity_cmd.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+# unstow the arm
+import time
+import rbd_spot
+from bosdyn.client.robot_command import blocking_stand
+
+TRANS_VEL = 0.5    # m/s
+ROT_VEL = 0.5    # rad/s
+
+def main():
+    conn = rbd_spot.SpotSDKConn(sdk_name="VelocityCmdClient",
+                                take_lease=True)
+    command_client = rbd_spot.body.create_client(conn)
+    blocking_stand(command_client, timeout_sec=10)
+
+    rbd_spot.body.velocityCommand(conn, command_client, TRANS_VEL, 0.0, 0.0, duration=1.0)
+    time.sleep(2)
+    rbd_spot.body.velocityCommand(conn, command_client, 0.0, -TRANS_VEL, 0.0, duration=1.0)
+    time.sleep(2)
+    rbd_spot.body.velocityCommand(conn, command_client, 0.0, 0.0, ROT_VEL, duration=1.0)
+    time.sleep(2)
+
+if __name__ == "__main__":
+    main()