adding velocityCommand in body.py; also make travel_params configurable

Author: zkytony

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

@@ -0,0 +1,38 @@
+# functions related to moving the robot's arm (including gripper)
+import time
+import bosdyn.client.lease
+from bosdyn.api import arm_command_pb2, geometry_pb2, robot_command_pb2
+from bosdyn.client import math_helpers
+from bosdyn.client.robot_command import (RobotCommandBuilder, RobotCommandClient,
+                                         block_until_arm_arrives, blocking_stand)
+from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME, ODOM_FRAME_NAME, get_a_tform_b
+
+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):
+    """
+    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 (assuming 10Hz command rate).
+    Returns:
+        RobotCommandResponse, float (time taken)
+    """
+    if mobility_params is None:
+        mobility_params = RobotCommandBuilder.mobility_params()
+    end_time = time.time() + cmd_duration
+    synchro_velocity_command = RobotCommandBuilder.synchro_velocity_command(
+        v_x=v_x, v_y=v_y, v_rot=v_rot, params=mobility_params)
+
+    with bosdyn.client.lease.LeaseKeepAlive(conn.lease_client, must_acquire=True):
+        _start_time = time.time()
+        command_id = command_client.robot_command(
+            synchro_velocity_command, end_time_secs=end_time,
+            timesync_endpoint=conn.timesync_endpoint)
+        end_time = time.time() - _start_time
+        return command_id, end_time

spot/ros_ws/src/rbd_spot_perception/src/rbd_spot_perception/graphnav.py

@@ -341,7 +341,8 @@ def listGraphWaypoints(graphnav_client):
         graph, localization_id)  # THIS FUNCTION DOES THE PRINTING.
 
 
-def navigateTo(conn, graphnav_client, goal, sleep=0.5, tolerance=None, speed="medium"):
+def navigateTo(conn, graphnav_client, goal, sleep=0.5, tolerance=None,
+               speed="medium", travel_params=None):
     """
     Navigate to a pose in the seed frame (i.e. global pose).
     Calls the NavigateToAnchor service. Blocking call until
@@ -363,6 +364,7 @@ def navigateTo(conn, graphnav_client, goal, sleep=0.5, tolerance=None, speed="me
             the goal in x, y, z axes. Sets the 'goal_waypoint_rt_seed_ewrt_seed_tolerance'
             parameter. Applicable only for seed frame goals.
         speed (str): "medium", "slow", "fast", or "default".
+        travel_params (TravelParams proto): Travel params for navigation. If provided, will override speed.
         probably don't need:
         +callback: function to be called per cycle+
         +callback_args+
@@ -393,15 +395,15 @@ def navigateTo(conn, graphnav_client, goal, sleep=0.5, tolerance=None, speed="me
     if tolerance is not None:
         goal_waypoint_rt_seed_ewrt_seed_tolerance = Vec3(x=tolerance[0], y=tolerance[1], z=tolerance[2])
 
-    travel_params = None
-    if speed not in {"default", "medium", "slow", "fast"}:
-        raise ValueError("Invalid speed for navigation:", speed)
-    if speed == "slow":
-        travel_params = graph_nav_pb2.TravelParams(velocity_limit=NAV_VELOCITY_LIMITS_SLOW)
-    elif speed == "medium":
-        travel_params = graph_nav_pb2.TravelParams(velocity_limit=NAV_VELOCITY_LIMITS_MEDIUM)
-    elif speed == "fast":
-        travel_params = graph_nav_pb2.TravelParams(velocity_limit=NAV_VELOCITY_LIMITS_FAST)
+    if travel_params is None:
+        if speed not in {"default", "medium", "slow", "fast"}:
+            raise ValueError("Invalid speed for navigation:", speed)
+        if speed == "slow":
+            travel_params = graph_nav_pb2.TravelParams(velocity_limit=NAV_VELOCITY_LIMITS_SLOW)
+        elif speed == "medium":
+            travel_params = graph_nav_pb2.TravelParams(velocity_limit=NAV_VELOCITY_LIMITS_MEDIUM)
+        elif speed == "fast":
+            travel_params = graph_nav_pb2.TravelParams(velocity_limit=NAV_VELOCITY_LIMITS_FAST)
 
     nav_to_cmd_id = None
     is_finished = False

spot/ros_ws/src/rbd_spot_robot/src/rbd_spot_robot/spot_sdk_conn.py

@@ -73,6 +73,10 @@ def lease(self):
     def clock_skew(self):
         return self.robot.time_sync.endpoint.clock_skew
 
+    @property
+    def timesync_endpoint(self):
+        return self.robot.time_sync.endpoint
+
     def spot_time_to_local(self, spot_timestamp):
         """
         Args: