Sending commands to the robot.

examples/inverse_kinematics/inverse_kinematics/send_inverse_kinematics_requests.py

@@ -7,6 +7,10 @@
 from utilities.tf_listener_wrapper import TFListenerWrapper
 from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME, ODOM_FRAME_NAME
 from utilities.simple_spot_commander import SimpleSpotCommander
+from spot_utilities.spot_basic import SpotBasic
+
+from spot_msgs.srv import Dock
+from std_srvs.srv import Trigger
 
 
 def cli() -> argparse.ArgumentParser:
@@ -16,7 +20,8 @@ def cli() -> argparse.ArgumentParser:
 
 
 def send_requests(robot_name: Optional[str] = None) -> bool:
-    # Set up basic ROS2 utilities for communicating with the driver
+
+    # Set up basic ROS2 utilities for communicating with the driver.
     node = ros_scope.node()
     if node is None:
         raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)")
@@ -27,33 +32,44 @@ def send_requests(robot_name: Optional[str] = None) -> bool:
     tf_listener = TFListenerWrapper(node)
     tf_listener.wait_for_a_tform_b(odom_frame_name, grav_aligned_body_frame_name)
 
-    robot = SimpleSpotCommander(robot_name, node)
+    robot = SpotBasic(robot_name)
 
-    # Claim robot
+    # Claim robot.
     logger.info("Claiming robot")
-    result = robot.command("claim")
-    if not result.success:
-        node.get_logger().error("Unable to claim robot message was " + result.message)
+    result = robot.claim()
+    if not result:
+        node.get_logger().error("Unable to claim robot")
         return False
     logger.info("Claimed robot")
 
-    # Stand the robot up.
+    # Power on robot.
     logger.info("Powering robot on")
-    result = robot.command("power_on")
-    if not result.success:
-        logger.error("Unable to power on robot message was " + result.message)
+    result = robot.power_on()
+    if not result:
+        logger.error("Unable to power on robot")
         return False
+
+    # Stand up robot.
     logger.info("Standing robot up")
-    result = robot.command("stand")
-    if not result.success:
-        logger.error("Robot did not stand message was " + result.message)
+    result = robot.stand()
+    if not result:
+        logger.error("Robot did not stand")
         return False
     logger.info("Successfully stood up.")
 
-    # TODO: send an inverse kinematic request.
-
+    # # Dock robot.
     # logger.info("docking robot")
-    #     self._robot.dock(527)
+    # result = robot.dock(527)
+    # if not result:
+    #     logger.error("Robot did not docked succesfully")
+    #     return False
+
+    # Power off robot.
+    logger.info("Powering robot off")
+    result = robot.power_off()
+    if not result:
+        logger.error("Unable to power off robot")
+        return False
 
     return True
 

examples/inverse_kinematics/setup.py

@@ -19,7 +19,7 @@
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
-            "send_inverse_kinematic_requests = inverse_kinematics.send_inverse_kinematic_requests:main",
+            "send_inverse_kinematics_requests = inverse_kinematics.send_inverse_kinematics_requests:main",
         ],
     },
 )