From 8dbfcb6fa296d2e6b194697b2385e036b86ec536 Mon Sep 17 00:00:00 2001 From: Zahi Kakish Date: Thu, 28 Mar 2024 16:56:23 -0600 Subject: [PATCH] v0.1.1 bug fixes, syntax fixes, and update to 4.0.0 --- pyproject.toml | 11 +- scripts/demo_arm.py | 49 ++--- scripts/demo_arm_no_dock.py | 117 ++++++++++ scripts/demo_autonomy.py | 58 ++--- scripts/demo_fiducial.py | 36 ++- scripts/demo_pose.py | 36 ++- scripts/demo_search.py | 205 ++++++++++++++++++ src/spot_bt/behaviors/actions/arm/__init__.py | 2 + src/spot_bt/behaviors/actions/arm/base.py | 18 +- src/spot_bt/behaviors/actions/arm/gripper.py | 28 ++- .../behaviors/actions/arm/trajectory.py | 93 ++++---- .../behaviors/actions/general/__init__.py | 1 - .../behaviors/actions/general/docking.py | 38 +++- .../behaviors/actions/general/power.py | 14 +- .../behaviors/actions/general/state.py | 24 +- .../behaviors/actions/movement/__init__.py | 1 + .../behaviors/actions/movement/move.py | 41 ++-- .../behaviors/actions/movement/pose.py | 7 +- .../behaviors/actions/movement/search.py | 112 ++++++++++ .../actions/perception/classifier.py | 22 +- .../behaviors/actions/perception/depth.py | 37 ++-- .../behaviors/actions/perception/fiducial.py | 18 +- .../behaviors/actions/perception/graph.py | 17 +- .../behaviors/actions/perception/image.py | 36 ++- .../behaviors/actions/perception/objects.py | 5 +- src/spot_bt/behaviors/actions/template.py | 2 + .../behaviors/conditions/general/__init__.py | 1 + .../behaviors/conditions/general/docking.py | 37 ++++ .../conditions/perception/fiducial.py | 11 +- .../behaviors/conditions/perception/graph.py | 4 +- src/spot_bt/cameras.py | 24 +- src/spot_bt/composites/parallel.py | 2 +- src/spot_bt/composites/selector.py | 40 ++-- src/spot_bt/composites/sequence.py | 69 ++---- src/spot_bt/data.py | 81 ++++--- src/spot_bt/tick.py | 2 + src/spot_bt/utils.py | 7 +- 37 files changed, 920 insertions(+), 386 deletions(-) create mode 100644 scripts/demo_arm_no_dock.py create mode 100644 scripts/demo_search.py create mode 100644 src/spot_bt/behaviors/actions/movement/search.py create mode 100644 src/spot_bt/behaviors/conditions/general/docking.py diff --git a/pyproject.toml b/pyproject.toml index 6e44dc2..364f2a4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -19,11 +19,12 @@ classifiers = [ dependencies = [ "aiortc", "black", - "bosdyn-api==3.3.2", - "bosdyn-choreography-client==3.3.2", - "bosdyn-client==3.3.2", - "bosdyn-core==3.3.2", - "bosdyn-mission==3.3.2", + "bosdyn-api==4.0.0", + "bosdyn-choreography-client==4.0.0", + "bosdyn-client==4.0.0", + "bosdyn-core==4.0.0", + "bosdyn-mission==4.0.0", + "graphviz", "numpy==1.24.1", "opencv-python==4.5.*", "Pillow", diff --git a/scripts/demo_arm.py b/scripts/demo_arm.py index 1f944d1..3a44f68 100644 --- a/scripts/demo_arm.py +++ b/scripts/demo_arm.py @@ -9,38 +9,32 @@ from bosdyn.client.math_helpers import SE3Pose import py_trees +from py_trees.composites import Sequence from spot_bt.data import Blackboards from spot_bt.data import ArmPose, ArmPoses -from spot_bt.behaviors.actions.general import ClaimLease, RobotPowerOff, RobotPowerOn from spot_bt.behaviors.actions.arm import ArmStow, ArmUnstow from spot_bt.behaviors.actions.arm import CloseGripper, OpenGripper -from spot_bt.behaviors.actions.arm import ArmTrajectory, ArmTrajectories +from spot_bt.behaviors.actions.arm import ArmTrajectories +from spot_bt.composites.selector import create_undock_selector from spot_bt.composites.sequence import create_dock_sequence -from spot_bt.composites.sequence import create_undock_sequence from spot_bt.tick import generic_pre_tick_handler - -def create_root() -> py_trees.composites.Sequence: +def create_root() -> Sequence: """Create the root for the Autonomy capability.""" - root = py_trees.composites.Sequence("DemoArm", memory=True) - - root.add_child(create_undock_sequence()) + root = Sequence("DemoArm", memory=True) root.add_children( [ - ClaimLease(name="claim_lease"), - RobotPowerOn(name="power_on"), + create_undock_selector(), ArmUnstow(name="unstow_arm"), - # ArmTrajectory(name="move_arm_to_postion"), - ArmTrajectories(name="move_arm_to_postions"), OpenGripper(name="open_gripper"), + ArmTrajectories(name="move_arm_to_postions"), CloseGripper(name="close_gripper"), ArmStow(name="stow_arm"), - RobotPowerOff(name="power_off"), + create_dock_sequence(), ] ) - root.add_child(create_dock_sequence()) py_trees.display.render_dot_tree(root) @@ -54,8 +48,7 @@ def main(): raise ValueError( "The SPOT_IP environment variable was not set." ) - else: - robot = sdk.create_robot(spot_ip) + robot = sdk.create_robot(spot_ip) bosdyn.client.util.authenticate(robot) robot.time_sync.wait_for_sync() @@ -65,32 +58,34 @@ def main(): blackboard = Blackboards() blackboard.state = py_trees.blackboard.Client(name="State") blackboard.state.register_key(key="robot", access=py_trees.common.Access.WRITE) - blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) - blackboard.state.register_key(key="pose", access=py_trees.common.Access.WRITE) blackboard.state.robot = robot + blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) blackboard.state.dock_id = 549 + blackboard.state.register_key(key="is_docked", access=py_trees.common.Access.WRITE) + blackboard.state.is_docked = True + blackboard.state.register_key( + key="is_gripper_open", access=py_trees.common.Access.WRITE + ) + blackboard.state.is_gripper_open = False + blackboard.arm = py_trees.blackboard.Client(name="Arm") - blackboard.arm.register_key(key="target", access=py_trees.common.Access.WRITE) blackboard.arm.register_key(key="command", access=py_trees.common.Access.WRITE) - # blackboard.arm.target = ArmPose( - # SE3Pose(x=1.0, y=0.0, z=0.5, rot=[1.0, 0.0, 0.0, 0.0]), - # False, - # ) + blackboard.arm.command = None + blackboard.arm.register_key(key="target", access=py_trees.common.Access.WRITE) blackboard.arm.target = ArmPoses([ ArmPose( - SE3Pose(x=1.0, y=0.0, z=0.0, rot=[1.0, 0.0, 0.0, 0.0]), + SE3Pose(x=0.9, y=0.0, z=0.0, rot=[1.0, 0.0, 0.0, 0.0]), False, ), ArmPose( - SE3Pose(x=1.0, y=0.5, z=0.0, rot=[1.0, 0.0, 0.0, 0.0]), + SE3Pose(x=0.9, y=0.5, z=0.0, rot=[1.0, 0.0, 0.0, 0.0]), False, ), ArmPose( - SE3Pose(x=1.0, y=0.0, z=0.5, rot=[1.0, 0.0, 0.0, 0.0]), + SE3Pose(x=0.9, y=0.0, z=0.5, rot=[1.0, 0.0, 0.0, 0.0]), False, ), ]) - blackboard.arm.command = None # Create and execute behavior tree lease_client = robot.ensure_client( diff --git a/scripts/demo_arm_no_dock.py b/scripts/demo_arm_no_dock.py new file mode 100644 index 0000000..22e0757 --- /dev/null +++ b/scripts/demo_arm_no_dock.py @@ -0,0 +1,117 @@ +"""spot_bt Arm Demonstration.""" +from __future__ import annotations + +import os +import time + +import bosdyn.client +import bosdyn.client.util +from bosdyn.client.math_helpers import SE3Pose + +import py_trees +from py_trees.composites import Sequence + +from spot_bt.data import Blackboards +from spot_bt.data import ArmPose, ArmPoses +from spot_bt.behaviors.actions.general import RobotPowerOff, RobotPowerOn +from spot_bt.behaviors.actions.arm import ArmStow, ArmUnstow +from spot_bt.behaviors.actions.arm import CloseGripper, OpenGripper +from spot_bt.behaviors.actions.arm import ArmTrajectories +from spot_bt.tick import generic_pre_tick_handler + + +def create_root() -> Sequence: + """Create the root for the Autonomy capability.""" + root = Sequence("DemoArm", memory=True) + root.add_children( + [ + RobotPowerOn(name="power_on"), + ArmUnstow(name="unstow_arm"), + OpenGripper(name="open_gripper"), + ArmTrajectories(name="move_arm_to_postions"), + CloseGripper(name="close_gripper"), + ArmStow(name="stow_arm"), + RobotPowerOff(name="power_off"), + ] + ) + py_trees.display.render_dot_tree(root) + + return root + + +def main(): + sdk = bosdyn.client.create_standard_sdk("spot_bt_fiducial_demo") + spot_ip = os.getenv("SPOT_IP") + if spot_ip is None: + raise ValueError( + "The SPOT_IP environment variable was not set." + ) + robot = sdk.create_robot(spot_ip) + bosdyn.client.util.authenticate(robot) + robot.time_sync.wait_for_sync() + + # Create behavior tree and blackboard + py_trees.logging.level = py_trees.logging.Level.DEBUG + py_trees.blackboard.Blackboard.enable_activity_stream(maximum_size=100) + blackboard = Blackboards() + blackboard.state = py_trees.blackboard.Client(name="State") + blackboard.state.register_key(key="robot", access=py_trees.common.Access.WRITE) + blackboard.state.robot = robot + blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) + blackboard.state.dock_id = 549 + blackboard.state.register_key(key="is_docked", access=py_trees.common.Access.WRITE) + blackboard.state.is_docked = True + blackboard.state.register_key( + key="is_gripper_open", access=py_trees.common.Access.WRITE + ) + blackboard.state.is_gripper_open = False + + blackboard.arm = py_trees.blackboard.Client(name="Arm") + blackboard.arm.register_key(key="command", access=py_trees.common.Access.WRITE) + blackboard.arm.command = None + blackboard.arm.register_key(key="target", access=py_trees.common.Access.WRITE) + blackboard.arm.target = ArmPoses([ + ArmPose( + SE3Pose(x=1.0, y=0.0, z=0.0, rot=[1.0, 0.0, 0.0, 0.0]), + False, + ), + ArmPose( + SE3Pose(x=1.0, y=0.5, z=0.0, rot=[1.0, 0.0, 0.0, 0.0]), + False, + ), + ArmPose( + SE3Pose(x=1.0, y=0.0, z=0.5, rot=[1.0, 0.0, 0.0, 0.0]), + False, + ), + ]) + + # Create and execute behavior tree + lease_client = robot.ensure_client( + bosdyn.client.lease.LeaseClient.default_service_name + ) + with bosdyn.client.lease.LeaseKeepAlive( + lease_client, must_acquire=True, return_at_exit=True + ): + # Create visitors + debug_visitor = py_trees.visitors.DebugVisitor() + + # Enable tree stewardship + root = create_root() + behavior_tree = py_trees.trees.BehaviourTree(root) + behavior_tree.add_pre_tick_handler(generic_pre_tick_handler) + behavior_tree.visitors.append(debug_visitor) + # behavior_tree.add_post_tick_handler(generic_post_tick_handler) + behavior_tree.setup(timeout=15) + root.setup_with_descendants() + + # Begin autonomy loop + while True: + try: + behavior_tree.tick() + time.sleep(0.1) + except KeyboardInterrupt: + break + + +if __name__ == "__main__": + main() diff --git a/scripts/demo_autonomy.py b/scripts/demo_autonomy.py index 4e83fbf..67c3f0a 100644 --- a/scripts/demo_autonomy.py +++ b/scripts/demo_autonomy.py @@ -9,53 +9,36 @@ import py_trees -from spot_bt.data import Blackboards -from spot_bt.data import Pose +from spot_bt.data import Blackboards, Pose from spot_bt.behaviors.actions.general import RobotState -from spot_bt.behaviors.actions.movement import MoveToFiducial -from spot_bt.behaviors.actions.movement import RobotPose -from spot_bt.behaviors.actions.movement import TurnInPlace -from spot_bt.behaviors.actions.perception import DetectFiducialMarkers -from spot_bt.behaviors.actions.perception import DetectWorldObjects -from spot_bt.composites.selector import create_generic_fiducial_selector +from spot_bt.behaviors.actions.movement import MoveToFiducial, RobotPose, TurnInPlace +from spot_bt.behaviors.actions.perception import DetectFiducialMarkers, DetectWorldObjects +from spot_bt.composites.selector import create_generic_fiducial_selector, create_undock_selector from spot_bt.composites.sequence import create_dock_sequence -from spot_bt.composites.sequence import create_undock_sequence from spot_bt.tick import generic_pre_tick_handler def create_root() -> py_trees.composites.Sequence: """Create the root for the Autonomy capability.""" root = py_trees.composites.Sequence("DemoAutonomy", memory=True) - get_robot_state = RobotState(name="Get Spot State") - set_robot_pose = RobotPose(name="Set Spot Pose") - turn_90_0 = TurnInPlace("Turn 0 -> 90") - turn_90_1 = TurnInPlace("Turn 90 -> 180") - turn_90_2 = TurnInPlace("Turn 180 -> 270") - turn_90_3 = TurnInPlace("Turn 270 -> 360") - detect_objects = DetectWorldObjects(name="Detect World Objects") - detect_fiducials = DetectFiducialMarkers(name="Detect Fiducials") - - root.add_child(create_undock_sequence()) - root.add_children( - [ - get_robot_state, - set_robot_pose, - turn_90_0, - turn_90_1, - turn_90_2, - turn_90_3, - detect_objects, - detect_fiducials, - ] - ) - root.add_child(create_generic_fiducial_selector()) root.add_children( [ + create_undock_selector(), + RobotState(name="Get Spot State"), + RobotPose(name="Set Spot Pose"), + TurnInPlace("Turn 0 -> 90"), + TurnInPlace("Turn 90 -> 180"), + TurnInPlace("Turn 180 -> 270"), + TurnInPlace("Turn 270 -> 360"), + DetectWorldObjects(name="Detect World Objects"), + DetectFiducialMarkers(name="Detect Fiducials"), + create_generic_fiducial_selector(), RobotState(name="Get Spot State"), MoveToFiducial(name="Move to Fiducial"), + create_dock_sequence(), ] ) - root.add_child(create_dock_sequence()) + py_trees.display.render_dot_tree(root) return root @@ -79,20 +62,23 @@ def main(): blackboard = Blackboards() blackboard.state = py_trees.blackboard.Client(name="State") blackboard.state.register_key(key="robot", access=py_trees.common.Access.WRITE) - blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) - blackboard.state.register_key(key="pose", access=py_trees.common.Access.WRITE) blackboard.state.robot = robot + blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) blackboard.state.dock_id = 549 + blackboard.state.register_key(key="pose", access=py_trees.common.Access.WRITE) blackboard.state.pose = Pose() blackboard.state.pose.set_pose(yaw=0.4, roll=0.0, pitch=0.0) + blackboard.state.register_key(key="is_docked", access=py_trees.common.Access.WRITE) + blackboard.state.is_docked = True + blackboard.perception = py_trees.blackboard.Client(name="Perception") blackboard.perception.register_key( key="fiducials", access=py_trees.common.Access.WRITE ) + blackboard.perception.fiducials = None blackboard.perception.register_key( key="world_objects", access=py_trees.common.Access.WRITE ) - blackboard.perception.fiducials = None blackboard.perception.world_objects = None # Create and execute behavior tree diff --git a/scripts/demo_fiducial.py b/scripts/demo_fiducial.py index a6cb1a3..333a654 100644 --- a/scripts/demo_fiducial.py +++ b/scripts/demo_fiducial.py @@ -9,35 +9,25 @@ import py_trees -from spot_bt.data import Blackboards -from spot_bt.data import Pose -from spot_bt.behaviors.actions.movement import MoveToFiducial -from spot_bt.behaviors.actions.movement import RobotPose -from spot_bt.composites.selector import create_generic_fiducial_selector +from spot_bt.data import Blackboards, Pose +from spot_bt.behaviors.actions.movement import MoveToFiducial, RobotPose +from spot_bt.composites.selector import create_generic_fiducial_selector, create_undock_selector from spot_bt.composites.sequence import create_dock_sequence -from spot_bt.composites.sequence import create_undock_sequence from spot_bt.tick import generic_pre_tick_handler def create_root() -> py_trees.composites.Sequence: """Create the root for the Autonomy capability.""" root = py_trees.composites.Sequence("DemoFiducial", memory=True) - set_robot_pose = RobotPose(name="Spot Pose") - - root.add_child(create_undock_sequence()) - root.add_children( - [ - set_robot_pose, - ] - ) - root.add_child(create_generic_fiducial_selector()) root.add_children( [ + create_undock_selector(), + RobotPose(name="Spot Pose"), + create_generic_fiducial_selector(), MoveToFiducial(name="Move to Fiducial"), + create_dock_sequence(), ] ) - root.add_child(create_dock_sequence()) - py_trees.display.render_dot_tree(root) return root @@ -50,8 +40,7 @@ def main(): raise ValueError( "The SPOT_IP environment variable was not set." ) - else: - robot = sdk.create_robot(spot_ip) + robot = sdk.create_robot(spot_ip) bosdyn.client.util.authenticate(robot) robot.time_sync.wait_for_sync() @@ -61,20 +50,23 @@ def main(): blackboard = Blackboards() blackboard.state = py_trees.blackboard.Client(name="State") blackboard.state.register_key(key="robot", access=py_trees.common.Access.WRITE) - blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) - blackboard.state.register_key(key="pose", access=py_trees.common.Access.WRITE) blackboard.state.robot = robot + blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) blackboard.state.dock_id = 549 + blackboard.state.register_key(key="is_docked", access=py_trees.common.Access.WRITE) + blackboard.state.is_docked = True + blackboard.state.register_key(key="pose", access=py_trees.common.Access.WRITE) blackboard.state.pose = Pose() blackboard.state.pose.set_pose(yaw=0.4, roll=0.0, pitch=0.0) + blackboard.perception = py_trees.blackboard.Client(name="Perception") blackboard.perception.register_key( key="fiducials", access=py_trees.common.Access.WRITE ) + blackboard.perception.fiducials = None blackboard.perception.register_key( key="world_objects", access=py_trees.common.Access.WRITE ) - blackboard.perception.fiducials = None blackboard.perception.world_objects = None # Create and execute behavior tree diff --git a/scripts/demo_pose.py b/scripts/demo_pose.py index 4500f96..7a7e394 100644 --- a/scripts/demo_pose.py +++ b/scripts/demo_pose.py @@ -9,32 +9,23 @@ import py_trees from spot_bt.data import Pose -from spot_bt.behaviors.actions.general import ClaimLease -from spot_bt.behaviors.actions.general import RobotDock -from spot_bt.behaviors.actions.general import RobotPowerOff -from spot_bt.behaviors.actions.general import RobotPowerOn -from spot_bt.behaviors.actions.general import RobotUndock +from spot_bt.composites.selector import create_undock_selector +from spot_bt.composites.sequence import create_dock_sequence from spot_bt.behaviors.actions.movement import RobotPose def create_root() -> py_trees.composites.Sequence: + """Create the root for the Autonomy capability.""" root = py_trees.composites.Sequence("DemoPose", memory=True) - claim_robot = ClaimLease(name="Claim Lease") - power_on_robot = RobotPowerOn(name="Power ON Spot") - undock_robot = RobotUndock(name="Undock Spot") - set_robot_pose = RobotPose(name="Spot Pose") - dock_robot = RobotDock(name="Dock Spot") - power_off_robot = RobotPowerOff(name="Power OFF Spot") root.add_children( [ - claim_robot, - power_on_robot, - undock_robot, - set_robot_pose, - dock_robot, - power_off_robot, + create_undock_selector(), + RobotPose(name="Spot Pose"), + create_dock_sequence(), ] ) + py_trees.display.render_dot_tree(root) + return root @@ -45,8 +36,7 @@ def main(): raise ValueError( "The SPOT_IP environment variable was not set." ) - else: - robot = sdk.create_robot(spot_ip) + robot = sdk.create_robot(spot_ip) bosdyn.client.util.authenticate(robot) robot.time_sync.wait_for_sync() @@ -55,12 +45,14 @@ def main(): py_trees.blackboard.Blackboard.enable_activity_stream(maximum_size=100) blackboard = py_trees.blackboard.Client(name="State") blackboard.register_key(key="robot", access=py_trees.common.Access.WRITE) - blackboard.register_key(key="pose", access=py_trees.common.Access.WRITE) - blackboard.register_key(key="dock_id", access=py_trees.common.Access.WRITE) blackboard.robot = robot + blackboard.register_key(key="dock_id", access=py_trees.common.Access.WRITE) + blackboard.dock_id = 549 + blackboard.register_key(key="is_docked", access=py_trees.common.Access.WRITE) + blackboard.is_docked = True + blackboard.register_key(key="pose", access=py_trees.common.Access.WRITE) blackboard.pose = Pose() blackboard.pose.set_pose(yaw=0.4, roll=0.0, pitch=0.0) - blackboard.dock_id = 549 # Create and execute behavior tree lease_client = robot.ensure_client( diff --git a/scripts/demo_search.py b/scripts/demo_search.py new file mode 100644 index 0000000..bf23c38 --- /dev/null +++ b/scripts/demo_search.py @@ -0,0 +1,205 @@ +"""spot_bt Search Demonstration""" +from __future__ import annotations + +import os +import time + +import bosdyn.client +import bosdyn.client.util + +import py_trees +from py_trees.composites import Selector, Sequence + +from spot_bt.data import Blackboards +from spot_bt.behaviors.actions.general import RobotState +from spot_bt.behaviors.actions.movement import MoveToFiducial, RandomSimpleSearch +from spot_bt.behaviors.actions.perception import DetectFiducialMarkers +from spot_bt.behaviors.conditions.perception import IsAnyFiducialMarkerDetected +from spot_bt.composites.selector import create_undock_selector +from spot_bt.composites.sequence import create_dock_sequence +from spot_bt.tick import generic_pre_tick_handler + + +class IsFiducialMoveComplete(py_trees.behaviour.Behaviour): + def __init__(self, name: str): + super().__init__(name) + self.blackboard = Blackboards() + self.fiducial_move_complete = False + + def initialise(self): + """Initialize robot and client objects for condition on first tick.""" + self.logger.debug(f" {self.name} [IsFiducialMoveComplete::initialise()]") + self.blackboard.state = self.attach_blackboard_client("State") + self.blackboard.state.register_key( + key="fiducial_move_complete", access=py_trees.common.Access.READ + ) + self.fiducial_move_complete = self.blackboard.state.fiducial_move_complete + + def update(self): + """Run the IsFiducialMoveComplete condition when ticked.""" + self.logger.debug(f" {self.name} [IsFiducialMoveComplete::update()]") + if self.fiducial_move_complete: + return py_trees.common.Status.SUCCESS + + return py_trees.common.Status.FAILURE + + def terminate(self, new_status: str): + """Terminate condition and save information.""" + self.logger.debug( + f" {self.name} [IsFiducialmoveComplete::terminate().terminate()]" + f"[{self.status}->{new_status}]" + ) + + +class MarkFiducialMoveComplete(py_trees.behaviour.Behaviour): + def __init__(self, name: str): + super().__init__(name) + self.blackboard = Blackboards() + self.fiducial_move_complete = False + + def initialise(self): + """Initialize robot and client objects for condition on first tick.""" + self.logger.debug(f" {self.name} [MarkFiducialMoveComplete::initialise()]") + self.blackboard.state = self.attach_blackboard_client("State") + self.blackboard.state.register_key( + key="fiducial_move_complete", access=py_trees.common.Access.WRITE + ) + + def update(self): + """Run the IsFiducialMoveComplete condition when ticked.""" + self.logger.debug(f" {self.name} [MarkFiducialMoveComplete::update()]") + self.blackboard.state.fiducial_move_complete = True + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status: str): + """Terminate condition and save information.""" + self.logger.debug( + f" {self.name} [MarkFiducialmoveComplete::terminate().terminate()]" + f"[{self.status}->{new_status}]" + ) + + +def create_detect_and_move_sb( + name="Move to Marker, if Detected", memory: bool = False +) -> Sequence: + """Create a Fiducial Detection Sub-Behavior""" + sequence = Sequence(name, memory) + sequence.add_children( + [ + DetectFiducialMarkers("Detect Fiducial Marker"), + IsAnyFiducialMarkerDetected("Is Any Fiducial Marker Detected?"), + MoveToFiducial("Move to Fiducial"), + MarkFiducialMoveComplete("Mark Completed"), + ] + ) + return sequence + + +def create_search_sb(name="Search for Fiducial", memory: bool = False) -> Sequence: + """Create Search Sub-Behavior.""" + sequence = Sequence(name, memory) + sequence.add_children( + [ + RobotState("Get Spot State"), + RandomSimpleSearch("Search Area"), + ] + ) + return sequence + + +def create_move_to_fiducial_ppa( + name: str = "Move to Fiducial PPA", memory: bool = False +) -> Selector: + """Create Move to Fiducial Precondition-Postcondition-Action.""" + ppa = Selector(name, memory) + ppa.add_children( + [ + IsFiducialMoveComplete("Is Move to Fiducial Complete?"), + create_detect_and_move_sb(), + create_search_sb(), + ] + ) + + return ppa + + +def create_root() -> Sequence: + """Create the root for the Autonomy capability.""" + root = Sequence("DemoSearch", memory=True) + root.add_children( + [ + create_undock_selector(), + create_move_to_fiducial_ppa(), + create_dock_sequence(), + ] + ) + py_trees.display.render_dot_tree(root) + + return root + + +def main(): + # Set up robot + sdk = bosdyn.client.create_standard_sdk("spot_bt_autonomy_demo") + spot_ip = os.getenv("SPOT_IP") + if spot_ip is None: + raise ValueError( + "The SPOT_IP environment variable was not set." + ) + robot = sdk.create_robot(spot_ip) + bosdyn.client.util.authenticate(robot) + robot.time_sync.wait_for_sync() + + # Create behavior tree and blackboard + py_trees.logging.level = py_trees.logging.Level.DEBUG + py_trees.blackboard.Blackboard.enable_activity_stream(maximum_size=100) + blackboard = Blackboards() + blackboard.state = py_trees.blackboard.Client(name="State") + blackboard.state.register_key(key="robot", access=py_trees.common.Access.WRITE) + blackboard.state.robot = robot + blackboard.state.register_key(key="dock_id", access=py_trees.common.Access.WRITE) + blackboard.state.dock_id = 549 + blackboard.state.register_key(key="is_docked", access=py_trees.common.Access.WRITE) + blackboard.state.is_docked = True + blackboard.state.register_key(key="move_duration", access=py_trees.common.Access.WRITE) + blackboard.state.move_duration = 3.0 + blackboard.state.register_key( + key="fiducial_move_complete", access=py_trees.common.Access.WRITE + ) + blackboard.state.fiducial_move_complete = False + + blackboard.perception = py_trees.blackboard.Client(name="Perception") + blackboard.perception.register_key( + key="fiducials", access=py_trees.common.Access.WRITE + ) + blackboard.perception.fiducials = None + + # Create and execute behavior tree + lease_client = robot.ensure_client( + bosdyn.client.lease.LeaseClient.default_service_name + ) + with bosdyn.client.lease.LeaseKeepAlive( + lease_client, must_acquire=True, return_at_exit=True + ): + # Create visitors + debug_visitor = py_trees.visitors.DebugVisitor() + + # Enable tree stewardship + root = create_root() + behavior_tree = py_trees.trees.BehaviourTree(root) + behavior_tree.add_pre_tick_handler(generic_pre_tick_handler) + behavior_tree.visitors.append(debug_visitor) + behavior_tree.setup(timeout=15) + root.setup_with_descendants() + + # Begin autonomy loop + while True: + try: + behavior_tree.tick() + time.sleep(0.1) + except KeyboardInterrupt: + break + + +if __name__ == "__main__": + main() diff --git a/src/spot_bt/behaviors/actions/arm/__init__.py b/src/spot_bt/behaviors/actions/arm/__init__.py index 8b08766..d562589 100755 --- a/src/spot_bt/behaviors/actions/arm/__init__.py +++ b/src/spot_bt/behaviors/actions/arm/__init__.py @@ -1 +1,3 @@ from spot_bt.behaviors.actions.arm.base import * +from spot_bt.behaviors.actions.arm.gripper import * +from spot_bt.behaviors.actions.arm.trajectory import * \ No newline at end of file diff --git a/src/spot_bt/behaviors/actions/arm/base.py b/src/spot_bt/behaviors/actions/arm/base.py index 34749e5..4239b7b 100755 --- a/src/spot_bt/behaviors/actions/arm/base.py +++ b/src/spot_bt/behaviors/actions/arm/base.py @@ -1,6 +1,10 @@ -from bosdyn.client.robot_command import block_until_arm_arrives -from bosdyn.client.robot_command import RobotCommandBuilder -from bosdyn.client.robot_command import RobotCommandClient +from __future__ import annotations + +from bosdyn.client.robot_command import ( + block_until_arm_arrives, + RobotCommandBuilder, + RobotCommandClient, +) import py_trees @@ -43,9 +47,9 @@ def update(self) -> py_trees.common.Status: stow_command_id = self.client.robot_command(stow) timeout = 3.0 - self.robot.logger.info("Stow command issued.") + self.logger.info("Stow command issued.") block_until_arm_arrives(self.client, stow_command_id, timeout) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -93,9 +97,9 @@ def update(self) -> py_trees.common.Status: timeout = 3.0 # Blocks until arm achieves a finishing state for specific command - self.robot.logger.info("Unstow command issued.") + self.logger.info("Unstow command issued.") block_until_arm_arrives(self.client, unstow_command_id, timeout) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/arm/gripper.py b/src/spot_bt/behaviors/actions/arm/gripper.py index 10a9c24..c953dda 100755 --- a/src/spot_bt/behaviors/actions/arm/gripper.py +++ b/src/spot_bt/behaviors/actions/arm/gripper.py @@ -1,6 +1,10 @@ -from bosdyn.client.robot_command import block_until_arm_arrives -from bosdyn.client.robot_command import RobotCommandBuilder -from bosdyn.client.robot_command import RobotCommandClient +from __future__ import annotations + +from bosdyn.client.robot_command import ( + block_until_arm_arrives, + RobotCommandBuilder, + RobotCommandClient, +) import py_trees @@ -25,6 +29,9 @@ def initialise(self): self.blackboard.state.register_key( key="robot", access=py_trees.common.Access.WRITE ) + self.blackboard.state.register_key( + key="is_gripper_open", access=py_trees.common.Access.WRITE + ) self.robot = self.blackboard.state.robot self.client = self.robot.ensure_client(RobotCommandClient.default_service_name) @@ -39,9 +46,11 @@ def update(self) -> py_trees.common.Status: command_id = self.client.robot_command(command) timeout = 3.0 - self.robot.logger.info("Close gripper command issued.") + self.logger.info("Close gripper command issued.") block_until_arm_arrives(self.client, command_id, timeout) - except: + self.blackboard.state.is_gripper_open = False + return py_trees.common.Status.SUCCESS + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE def terminate(self, new_status: str): @@ -70,6 +79,9 @@ def initialise(self): self.blackboard.state.register_key( key="robot", access=py_trees.common.Access.WRITE ) + self.blackboard.state.register_key( + key="is_gripper_open", access=py_trees.common.Access.WRITE + ) self.robot = self.blackboard.state.robot self.client = self.robot.ensure_client(RobotCommandClient.default_service_name) @@ -85,9 +97,11 @@ def update(self) -> py_trees.common.Status: command_id = self.client.robot_command(command) timeout = 3.0 - self.robot.logger.info("Open gripper command issued.") + self.logger.info("Open gripper command issued.") block_until_arm_arrives(self.client, command_id, timeout) - except: + self.blackboard.state.is_gripper_open = True + return py_trees.common.Status.SUCCESS + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE def terminate(self, new_status: str): diff --git a/src/spot_bt/behaviors/actions/arm/trajectory.py b/src/spot_bt/behaviors/actions/arm/trajectory.py index 76d8d7b..0d198e2 100755 --- a/src/spot_bt/behaviors/actions/arm/trajectory.py +++ b/src/spot_bt/behaviors/actions/arm/trajectory.py @@ -1,17 +1,18 @@ -from bosdyn.api import arm_command_pb2 -from bosdyn.api import geometry_pb2 +from __future__ import annotations +from bosdyn.api import arm_command_pb2, geometry_pb2 from bosdyn.client import math_helpers -from bosdyn.client.frame_helpers import get_a_tform_b -from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME -from bosdyn.client.frame_helpers import ODOM_FRAME_NAME -from bosdyn.client.robot_command import RobotCommandBuilder -from bosdyn.client.robot_command import RobotCommandClient +from bosdyn.client.frame_helpers import ( + get_a_tform_b, + GRAV_ALIGNED_BODY_FRAME_NAME, + ODOM_FRAME_NAME, +) +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_state import RobotStateClient import py_trees -from spot_bt.data import Blackboards -from spot_bt.data import ArmPose, ArmPoses +from spot_bt.data import ArmPose, ArmPoses, Blackboards class ArmTrajectory(py_trees.behaviour.Behaviour): @@ -21,27 +22,36 @@ def __init__(self, name: str): super().__init__(name) self.blackboard = Blackboards() self.robot = None - self.client = None - self.state = None + self.cmd_client = None + self.state_client = None self.feedback = None self.target: ArmPose | None = None - self.option = "state" + self.cmd_id = None + + def setup(self, **kwargs): + """Setup ArmTrajectory behavior before initialization.""" + self.logger.debug(f" {self.name} [ArmTrajectory::setup()]") def initialise(self): """Initialize robot object and client behavior for first tick.""" self.logger.debug(f" {self.name} [ArmTrajectory::initialise()]") self.blackboard.state = self.attach_blackboard_client("State") self.blackboard.state.register_key(key="robot", access=py_trees.common.Access.WRITE) + self.blackboard.state.register_key( + key="is_gripper_open", access=py_trees.common.Access.READ + ) self.blackboard.arm = self.attach_blackboard_client("Arm") self.blackboard.arm.register_key( key="target", access=py_trees.common.Access.WRITE ) self.robot = self.blackboard.state.robot self.target = self.blackboard.arm.target - self.client = self.robot.ensure_client(RobotCommandClient.default_service_name) + self.cmd_client = self.robot.ensure_client(RobotCommandClient.default_service_name) + self.state_client = self.robot.ensure_client(RobotStateClient.default_service_name) def update(self) -> py_trees.common.Status: """Run the ArmTrajectory behavior when ticked.""" + # pylint: disable=no-member self.logger.debug(f" {self.name} [ArmTrajectory::update()]") try: if isinstance(self.target, ArmPoses): @@ -62,7 +72,7 @@ def update(self) -> py_trees.common.Status: position=hand_ewrt_flat_body, rotation=flat_body_Q_hand ) - robot_state = self.client.get_robot_state() + robot_state = self.state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, @@ -89,10 +99,12 @@ def update(self) -> py_trees.common.Status: ) # Make the open gripper RobotCommand - # TODO Fix this. This should be what the current state of the gripper is. - gripper_command = ( - RobotCommandBuilder.claw_gripper_open_fraction_command(1.0) - ) + if self.blackboard.state.is_gripper_open: + gripper_command = ( + RobotCommandBuilder.claw_gripper_open_fraction_command(1.0) + ) + else: + gripper_command = RobotCommandBuilder.claw_gripper_close_command() # Combine the arm and gripper commands into one RobotCommand command = RobotCommandBuilder.build_synchro_command( @@ -100,11 +112,11 @@ def update(self) -> py_trees.common.Status: ) # Send the request - cmd_id = self.client.robot_command(command) + self.cmd_id = self.cmd_client.robot_command(command) # Wait until the arm arrives at the goal. - self.feedback = self.client.robot_command_feedback(cmd_id) - self.robot.logger.info( + self.feedback = self.cmd_client.robot_command_feedback(self.cmd_id) + self.logger.info( "Distance to go: " f"{self.feedback.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.measured_pos_distance_to_goal:.2f} meters" f", {self.feedback.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.measured_rot_distance_to_goal:.2f} radians" @@ -114,7 +126,7 @@ def update(self) -> py_trees.common.Status: self.feedback.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE ): - self.robot.logger.info("Move complete.") + self.logger.info("Move complete.") return py_trees.common.Status.SUCCESS return py_trees.common.Status.RUNNING @@ -137,14 +149,18 @@ def __init__(self, name: str): super().__init__(name) self.blackboard = Blackboards() self.robot = None - self.client = None - self.state = None + self.cmd_client = None + self.state_client = None self.feedback = None self.target: ArmPoses | None = None self.current_target = None self.possible_targets = None self.count = 0 - self.option = "state" + self.cmd_id = None + + def setup(self, **kwargs): + """Setup ArmTrajectories behavior before initialization.""" + self.logger.debug(f" {self.name} [ArmTrajectories::setup()]") def initialise(self): """Initialize robot object and client behavior for first tick.""" @@ -157,7 +173,9 @@ def initialise(self): ) self.robot = self.blackboard.state.robot self.target = self.blackboard.arm.target - self.client = self.robot.ensure_client(RobotCommandClient.default_service_name) + self.cmd_client = self.robot.ensure_client(RobotCommandClient.default_service_name) + self.state_client = self.robot.ensure_client(RobotStateClient.default_service_name) + self.count = 0 if isinstance(self.target, ArmPose): self.possible_targets = [self.target.pose] self.logger.warning("Only one pose given to ArmTrajectories") @@ -166,13 +184,14 @@ def initialise(self): def update(self) -> py_trees.common.Status: """Run the ArmTrajectories behavior when ticked.""" + # pylint: disable=no-member self.logger.debug(f" {self.name} [ArmTrajectories::update()]") try: - if self.feedback is None: self.current_target = self.possible_targets[self.count] # Make the arm pose RobotCommand - # Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame). + # Build a position to move the arm to (in meters, relative to and expressed in the + # gravity aligned body frame). hand_ewrt_flat_body = self.current_target.hand_pose() # Rotation as a quaternion @@ -182,7 +201,7 @@ def update(self) -> py_trees.common.Status: position=hand_ewrt_flat_body, rotation=flat_body_Q_hand ) - robot_state = self.client.get_robot_state() + robot_state = self.state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, @@ -220,11 +239,11 @@ def update(self) -> py_trees.common.Status: ) # Send the request - cmd_id = self.client.robot_command(command) + self.cmd_id = self.cmd_client.robot_command(command) # Wait until the arm arrives at the goal. - self.feedback = self.client.robot_command_feedback(cmd_id) - self.robot.logger.info( + self.feedback = self.cmd_client.robot_command_feedback(self.cmd_id) + self.logger.info( "Distance to go: " f"{self.feedback.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.measured_pos_distance_to_goal:.2f} meters" f", {self.feedback.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.measured_rot_distance_to_goal:.2f} radians" @@ -234,14 +253,14 @@ def update(self) -> py_trees.common.Status: self.feedback.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE ): - if self.count > len(self.possible_targets): - self.robot.logger.info("All moves complete!") - return py_trees.common.Status.SUCCESS - - self.robot.logger.info("Move complete.") + self.logger.info("Move complete.") self.count += 1 self.feedback = None + if self.count >= len(self.possible_targets): + self.logger.info("All moves complete!") + return py_trees.common.Status.SUCCESS + return py_trees.common.Status.RUNNING except: # pylint: disable=bare-except diff --git a/src/spot_bt/behaviors/actions/general/__init__.py b/src/spot_bt/behaviors/actions/general/__init__.py index 888bd06..f81f5a4 100755 --- a/src/spot_bt/behaviors/actions/general/__init__.py +++ b/src/spot_bt/behaviors/actions/general/__init__.py @@ -1,4 +1,3 @@ from spot_bt.behaviors.actions.general.docking import * from spot_bt.behaviors.actions.general.power import * -from spot_bt.behaviors.actions.general.search import * from spot_bt.behaviors.actions.general.state import * \ No newline at end of file diff --git a/src/spot_bt/behaviors/actions/general/docking.py b/src/spot_bt/behaviors/actions/general/docking.py index cee51fc..7221fcb 100755 --- a/src/spot_bt/behaviors/actions/general/docking.py +++ b/src/spot_bt/behaviors/actions/general/docking.py @@ -1,7 +1,7 @@ -from bosdyn.client.docking import blocking_dock_robot -from bosdyn.client.docking import blocking_undock -from bosdyn.client.robot_command import blocking_stand -from bosdyn.client.robot_command import RobotCommandClient +from __future__ import annotations + +from bosdyn.client.docking import blocking_dock_robot, blocking_undock +from bosdyn.client.robot_command import blocking_stand, RobotCommandClient import py_trees @@ -16,6 +16,10 @@ def __init__(self, name: str): self.client = None self.dock_id = None + def setup(self, **kwargs): + """Setup RobotDock behavior before initialization.""" + self.logger.debug(f" {self.name} [RobotDock::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [RobotDock::initialise()]") @@ -26,6 +30,9 @@ def initialise(self): self.blackboard.state.register_key( key="dock_id", access=py_trees.common.Access.READ ) + self.blackboard.state.register_key( + key="is_docked", access=py_trees.common.Access.WRITE + ) self.robot = self.blackboard.state.robot self.client = self.robot.ensure_client(RobotCommandClient.default_service_name) self.dock_id = int(self.blackboard.state.dock_id) @@ -37,9 +44,10 @@ def update(self) -> py_trees.common.Status: # NOTE: Make sure the robot is powered on first! blocking_stand(self.client) blocking_dock_robot(self.robot, self.dock_id) - self.logger.debug("\t\tROBOT DOCKED!") - except: - self.logger.debug("\t\tROBOT FAILED TO DOCK!") + self.blackboard.state.is_docked = True + self.logger.debug("\t\tRobot Docked!") + except: # pylint: disable=bare-except + self.logger.debug("\t\tRobot Failed to Dock!") return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -59,13 +67,20 @@ def __init__(self, name: str): self.robot = None self.dock_id = None + def setup(self, **kwargs): + """Setup RobotUndock behavior before initialization.""" + self.logger.debug(f" {self.name} [RobotUndock::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [RobotUndock::initialise()]") - self.blackboard.state = self.attach_blackboard_client("Spot State") + self.blackboard.state = self.attach_blackboard_client("State") self.blackboard.state.register_key( key="robot", access=py_trees.common.Access.WRITE ) + self.blackboard.state.register_key( + key="is_docked", access=py_trees.common.Access.WRITE + ) self.robot = self.blackboard.state.robot def update(self) -> py_trees.common.Status: @@ -74,9 +89,10 @@ def update(self) -> py_trees.common.Status: try: # NOTE: Make sure the robot is powered on first! blocking_undock(self.robot) - self.logger.debug("\t\tROBOT UNDOCKED!") - except: - self.logger.debug("\t\tROBOT FAILED TO UNDOCK!") + self.blackboard.state.is_docked = False + self.logger.debug("\t\tRobot Undocked!") + except: # pylint: disable=bare-except + self.logger.debug("\t\tRobot Failed to Undock!") return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/general/power.py b/src/spot_bt/behaviors/actions/general/power.py index 6f8f11b..618d8a1 100755 --- a/src/spot_bt/behaviors/actions/general/power.py +++ b/src/spot_bt/behaviors/actions/general/power.py @@ -1,4 +1,6 @@ """Robot power function related behaviors""" +from __future__ import annotations + import py_trees from spot_bt.data import Blackboards @@ -12,6 +14,10 @@ def __init__(self, name: str): self.blackboard = Blackboards() self.robot = None + def setup(self, **kwargs): + """Setup RobotPowerOn behavior before initialization.""" + self.logger.debug(f" {self.name} [RobotPowerOn::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [RobotPose::initialise()]") @@ -27,7 +33,7 @@ def update(self) -> py_trees.common.Status: try: self.robot.power_on(timeout_sec=20) assert self.robot.is_powered_on(), "Robot power on failed!" - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -48,6 +54,10 @@ def __init__(self, name: str): self.blackboard = Blackboards() self.robot = None + def setup(self, **kwargs): + """Setup RobotPowerOff behavior before initialization.""" + self.logger.debug(f" {self.name} [RobotPowerOff::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [RobotPose::initialise()]") @@ -63,7 +73,7 @@ def update(self) -> py_trees.common.Status: try: self.robot.power_off(cut_immediately=False, timeout_sec=20) assert not self.robot.is_powered_on(), "Robot power off failed!" - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/general/state.py b/src/spot_bt/behaviors/actions/general/state.py index af64956..f7483e0 100755 --- a/src/spot_bt/behaviors/actions/general/state.py +++ b/src/spot_bt/behaviors/actions/general/state.py @@ -1,4 +1,6 @@ """Robot state related behaviors.""" +from __future__ import annotations + from bosdyn.client.robot_state import RobotStateClient import py_trees @@ -19,6 +21,10 @@ def __init__(self, name: str): self.state = None self.option = "state" + def setup(self, **kwargs): + """Setup RobotState behavior before initialization.""" + self.logger.debug(f" {self.name} [RobotState::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [RobotState::initialise()]") @@ -38,10 +44,10 @@ def update(self) -> py_trees.common.Status: if self.option == "hardware": self.state = self.client.get_hardware_config_with_link_info() return py_trees.common.Status.SUCCESS - elif self.option == "metrics": + if self.option == "metrics": self.state = self.client.get_robot_metrics() return py_trees.common.Status.SUCCESS - elif self.option == "state": + if self.option == "state": self.state = self.client.get_robot_state() return py_trees.common.Status.SUCCESS @@ -70,6 +76,10 @@ def __init__(self, name: str): self.check = None self.option = "state" + def setup(self, **kwargs): + """Setup RobotStateAsync behavior before initialization.""" + self.logger.debug(f" {self.name} [RobotStateAsync::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [RobotStateAsync::initialise()]") @@ -97,9 +107,9 @@ def update(self) -> py_trees.common.Status: if not self.check.done(): return py_trees.common.Status.RUNNING - else: - self.state = self.check.results() - return py_trees.common.Status.SUCCESS + + self.state = self.check.results() + return py_trees.common.Status.SUCCESS except KeyError: return py_trees.common.Status.FAILURE @@ -121,6 +131,10 @@ def __init__(self, name: str): self.blackboard = Blackboards() self.finished = None + def setup(self, **kwargs): + """Setup FinishAutonomy behavior before initialization.""" + self.logger.debug(f" {self.name} [FinishAutonomy::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [FinishAutonomy::initialise()]") diff --git a/src/spot_bt/behaviors/actions/movement/__init__.py b/src/spot_bt/behaviors/actions/movement/__init__.py index 0ca8cc7..9d746f8 100755 --- a/src/spot_bt/behaviors/actions/movement/__init__.py +++ b/src/spot_bt/behaviors/actions/movement/__init__.py @@ -1,2 +1,3 @@ from spot_bt.behaviors.actions.movement.move import * from spot_bt.behaviors.actions.movement.pose import * +from spot_bt.behaviors.actions.movement.search import * \ No newline at end of file diff --git a/src/spot_bt/behaviors/actions/movement/move.py b/src/spot_bt/behaviors/actions/movement/move.py index 0d50dd4..58ee39a 100755 --- a/src/spot_bt/behaviors/actions/movement/move.py +++ b/src/spot_bt/behaviors/actions/movement/move.py @@ -1,25 +1,25 @@ """Robot Motion behaviors.""" +from __future__ import annotations + import time -from bosdyn.api import geometry_pb2 from bosdyn.api.basic_command_pb2 import RobotCommandFeedbackStatus from bosdyn.api.spot import robot_command_pb2 -from bosdyn.client.frame_helpers import get_a_tform_b -from bosdyn.client.frame_helpers import get_vision_tform_body -from bosdyn.client.frame_helpers import VISION_FRAME_NAME -from bosdyn.client.robot_command import RobotCommandBuilder -from bosdyn.client.robot_command import RobotCommandClient +from bosdyn.client.frame_helpers import ( + get_a_tform_b, + get_vision_tform_body, + VISION_FRAME_NAME, + ODOM_FRAME_NAME, +) +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient from bosdyn.client.robot_state import RobotStateClient import numpy as np import py_trees -from spot_bt.data import Blackboards -from spot_bt.data import BodyTrajectory -from spot_bt.data import Pose -from spot_bt.utils import get_desired_angle -from spot_bt.utils import get_default_mobility_parameters +from spot_bt.data import Blackboards, BodyTrajectory, Pose +from spot_bt.utils import get_desired_angle, get_default_mobility_parameters class MoveToFiducial(py_trees.behaviour.Behaviour): @@ -34,6 +34,10 @@ def __init__(self, name: str): self.dock_id = None self.state = None + def setup(self, **kwargs): + """Setup MoveToFiducial behavior before initialization.""" + self.logger.debug(f" {self.name} [MoveToFiducial::setup()]") + def initialise(self): """Initialize robot and client objects for behavior on first tick.""" self.logger.debug(f" {self.name} [MoveToFiducial::initialise()]") @@ -59,6 +63,7 @@ def initialise(self): def update(self) -> py_trees.common.Status: """Run the MoveToFiducial behavior when ticked.""" + # pylint: disable=no-member self.logger.debug(f" {self.name} [MoveToFiducial::update()]") if self.cmd_id is None: if len(self.fiducials) > 1: @@ -126,7 +131,7 @@ def update(self) -> py_trees.common.Status: def terminate(self, new_status: str): """Terminate behavior and save information.""" self.logger.debug( - f" {self.name} [MoveToFiducial::terminate().terminate()]" + + f" {self.name} [MoveToFiducial::terminate().terminate()]" f"[{self.status}->{new_status}]" ) @@ -140,6 +145,10 @@ def __init__(self, name: str): self.pose: Pose = None self.cmd_id = None + def setup(self, **kwargs): + """Setup TurnInPlace behavior before initialization.""" + self.logger.debug(f" {self.name} [TurnInPlace::setup()]") + def initialise(self): """Initialize robot and client objects for behavior on first tick.""" self.logger.debug(f" {self.name} [TurnInPlace::initialise()]") @@ -157,13 +166,15 @@ def initialise(self): def update(self) -> py_trees.common.Status: """Run the TurnInPlace behavior when ticked.""" + # pylint: disable=no-member self.logger.debug(f" {self.name} [TurnInPlace::update()]") try: if self.cmd_id is None: transform = self.client["state"].get_robot_state().kinematic_state.transforms_snapshot # TODO Need better way of inputing angle. cmd = BodyTrajectory().create_rotation_trajectory_command( - transform, 0, 0, 90 + # transform, 0, 0, 90 + transform, 90.0, ODOM_FRAME_NAME ) # TODO Figure out a way to vary time depending on angle size. self.cmd_id = self.client["command"].robot_command( @@ -184,12 +195,12 @@ def update(self) -> py_trees.common.Status: return py_trees.common.Status.RUNNING - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE def terminate(self, new_status: str): """Terminate behavior and save information.""" self.logger.debug( - f" {self.name} [TurnInPlace::terminate().terminate()]" + + f" {self.name} [TurnInPlace::terminate().terminate()]" f"[{self.status}->{new_status}]" ) diff --git a/src/spot_bt/behaviors/actions/movement/pose.py b/src/spot_bt/behaviors/actions/movement/pose.py index 06aad57..212da3f 100755 --- a/src/spot_bt/behaviors/actions/movement/pose.py +++ b/src/spot_bt/behaviors/actions/movement/pose.py @@ -1,4 +1,6 @@ """Robot Pose related behaviors""" +from __future__ import annotations + import time from bosdyn.client.robot_command import RobotCommandClient @@ -42,13 +44,12 @@ def update(self) -> py_trees.common.Status: self.logger.debug(f" {self.name} [RobotPose::update()]") try: assert self.robot.is_powered_on(), "Robot power on failed." - cmd = self.pose.create_command() self.client.robot_command(cmd) self.pose.mark() time.sleep(3) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -57,6 +58,6 @@ def terminate(self, new_status: str): """Terminate behavior and save information.""" self.blackboard.state.pose = self.pose self.logger.debug( - f" {self.name} [RobotPose::terminate().terminate()]" + + f" {self.name} [RobotPose::terminate().terminate()]" f"[{self.status}->{new_status}]" ) diff --git a/src/spot_bt/behaviors/actions/movement/search.py b/src/spot_bt/behaviors/actions/movement/search.py new file mode 100644 index 0000000..2963ede --- /dev/null +++ b/src/spot_bt/behaviors/actions/movement/search.py @@ -0,0 +1,112 @@ +"""Robot Search behaviors.""" +from __future__ import annotations + +import random +import time + +from bosdyn.api.basic_command_pb2 import RobotCommandFeedbackStatus +from bosdyn.client.frame_helpers import ODOM_FRAME_NAME +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_state import RobotStateClient + +import py_trees + +from spot_bt.data import Blackboards, BodyTrajectory +from spot_bt.utils import get_default_mobility_parameters + + +class RandomSimpleSearch(py_trees.behaviour.Behaviour): + def __init__(self, name: str): + super().__init__(name) + self.blackboard = Blackboards() + self.robot = None + self.client = {} + self.cmd_id = None + self.dock_id = None + self.state = None + self.move_duration = None + self.is_rotating = True + self.is_moving = False + self.done = False + + def initialise(self): + """Initialize robot and client objects for behavior on first tick.""" + self.logger.debug(f" {self.name} [RandomSearch::initialise()]") + self.blackboard.state = self.attach_blackboard_client("State") + self.blackboard.state.register_key( + key="robot", access=py_trees.common.Access.WRITE + ) + self.blackboard.state.register_key( + key="state", access=py_trees.common.Access.WRITE + ) + self.blackboard.state.register_key( + key="move_duration", access=py_trees.common.Access.READ + ) + self.robot = self.blackboard.state.robot + self.state = self.blackboard.state.state + self.move_duration = self.blackboard.state.move_duration + self.client["command"] = self.robot.ensure_client(RobotCommandClient.default_service_name) + self.client["state"] = self.robot.ensure_client(RobotStateClient.default_service_name) + self.is_rotating = True + self.is_moving = False + self.cmd_id = None + + def update(self) -> py_trees.common.Status: + """Run the RandomSearch behavior when ticked.""" + # pylint: disable=no-member + self.logger.debug(f" {self.name} [RandomSearch::update()]") + if self.cmd_id is None and self.is_moving: + # Set mobility parameters + # TODO: Make this into a static function possibly. + mobility_parameters = get_default_mobility_parameters() + + # Create command and send + cmd = RobotCommandBuilder.synchro_velocity_command( + v_x=0.5, v_y=0.0, v_rot=0.0, params=mobility_parameters + ) + self.cmd_id = self.client["command"].robot_command( + command=cmd, end_time_secs=(time.time() + self.move_duration) + ) + + if self.cmd_id is None and self.is_rotating: + transform = self.client["state"].get_robot_state().kinematic_state.transforms_snapshot + cmd = BodyTrajectory().create_rotation_trajectory_command( + transform, 45.0 * random.uniform(-1.0, 1.0), ODOM_FRAME_NAME + ) + self.cmd_id = self.client["command"].robot_command( + command=cmd, end_time_secs=(time.time() + self.move_duration) + ) + + feedback = self.client["command"].robot_command_feedback(self.cmd_id) + mobility_feedback = feedback.feedback.synchronized_feedback.mobility_command_feedback + if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING: + self.logger.error(f"Failed to search! Status: {mobility_feedback.status}") + return py_trees.common.Status.FAILURE + + trajectory_feedback = mobility_feedback.se2_trajectory_feedback + if (trajectory_feedback.status == trajectory_feedback.STATUS_AT_GOAL and + trajectory_feedback.body_movement_status == trajectory_feedback.BODY_STATUS_SETTLED): + self.logger.info("Finished searching!") + if self.is_rotating: + self.is_moving = True + self.is_rotating = False + self.cmd_id = None + else: + self.is_moving = False + self.is_rotating = False + self.cmd_id = None + self.done = True + + if self.done: + # TODO: Change this to a success status + return py_trees.common.Status.FAILURE + + return py_trees.common.Status.RUNNING + + + def terminate(self, new_status: str): + """Terminate behavior and save information.""" + self.logger.debug( + f" {self.name} [RandomSearch::terminate().terminate()]" + f"[{self.status}->{new_status}]" + ) diff --git a/src/spot_bt/behaviors/actions/perception/classifier.py b/src/spot_bt/behaviors/actions/perception/classifier.py index 3f4a173..229355b 100755 --- a/src/spot_bt/behaviors/actions/perception/classifier.py +++ b/src/spot_bt/behaviors/actions/perception/classifier.py @@ -1,9 +1,11 @@ """ML Classifier related behaviors.""" +from __future__ import annotations + import cv2 import py_trees -import torch +# import torch from spot_bt.data import Blackboards @@ -14,20 +16,20 @@ def __init__(self, name: str, path_to_model: str, first_load: bool = False): self.blackboard = Blackboards() self.classifications = [] self.first_load = first_load - self.image = None + self.images = None self.path_to_model = path_to_model self.model = None def setup(self, **kwargs): """Setup model inference.""" self.logger.debug(f"\t{self.name} [ClassifyObjectsInImage::setup()]") - try: - if self.first_load: - self.model = torch.load(self.path_to_model) - except FileNotFoundError as e: - self.logger.error( - f"{e.args} Could not find file in path {self.path_to_model}" - ) + # try: + # if self.first_load: + # self.model = torch.load(self.path_to_model) + # except FileNotFoundError as e: + # self.logger.error( + # f"{e.args} Could not find file in path {self.path_to_model}" + # ) def initialise(self): """Initialize robot object and client for behavior on first tick.""" @@ -54,7 +56,7 @@ def update(self) -> py_trees.common.Status: output = self.model(image) self.classifications.append(output) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/perception/depth.py b/src/spot_bt/behaviors/actions/perception/depth.py index d1ff8b7..43590ec 100644 --- a/src/spot_bt/behaviors/actions/perception/depth.py +++ b/src/spot_bt/behaviors/actions/perception/depth.py @@ -1,21 +1,18 @@ -import cv2 +from __future__ import annotations -from bosdyn.api import image_pb2 -from bosdyn.client.image import build_image_request -from bosdyn.client.image import ImageClient +from bosdyn.client.image import build_image_request, ImageClient import py_trees import numpy as np -from scipy import ndimage - from spot_bt.data import Blackboards -from spot_bt.cameras import get_encoding_for_pixel_format_string -from spot_bt.cameras import IMAGE_ROTATION_ANGLES -from spot_bt.cameras import DEPTH_CAMERA_OPTIONS -from spot_bt.cameras import DEPTH_CAMERA_OPTIONS_WITH_ARM -from spot_bt.cameras import pixel_format_string_to_enum +from spot_bt.cameras import ( + get_encoding_for_pixel_format_string, + DEPTH_CAMERA_OPTIONS, + DEPTH_CAMERA_OPTIONS_WITH_ARM, + pixel_format_string_to_enum, +) class SwitchDepthCamera(py_trees.behaviour.Behaviour): @@ -29,6 +26,10 @@ def __init__(self, name: str, use_arm: bool = False): else: self.image_sources = DEPTH_CAMERA_OPTIONS + def setup(self, **kwargs): + """Setup SwitchDepthCamera behavior before initialization.""" + self.logger.debug(f" {self.name} [SwitchDepthCamera::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [SwitchDepthCamera::initialise()]") @@ -56,9 +57,9 @@ def update(self) -> py_trees.common.Status: self.camera = self.image_sources[idx + 1] return py_trees.common.Status.SUCCESS - except: - return py_trees.common.Status.FAILURE + except: # pylint: disable=bare-except + return py_trees.common.Status.FAILURE def terminate(self, new_status: str): """Terminate behavior and save information.""" @@ -85,6 +86,10 @@ def __init__( self.pixel_format = pixel_format_string_to_enum(pixel_format) self.encoding_data = get_encoding_for_pixel_format_string(pixel_format) + def setup(self, **kwargs): + """Setup TakeDepthImage behavior before initialization.""" + self.logger.debug(f" {self.name} [TakeDepthImage::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [TakeDepthImage::initialise()]") @@ -121,7 +126,7 @@ def update(self) -> py_trees.common.Status: # Save image self.image = cv_depth - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -154,6 +159,10 @@ def __init__( else: self.image_sources = DEPTH_CAMERA_OPTIONS + def setup(self, **kwargs): + """Setup TakeDepthImageAllCameras behavior before initialization.""" + self.logger.debug(f" {self.name} [TakeDepthImageAllCameras::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [TakeDepthImageAllCameras::initialise()]") diff --git a/src/spot_bt/behaviors/actions/perception/fiducial.py b/src/spot_bt/behaviors/actions/perception/fiducial.py index 08f7619..4f49456 100755 --- a/src/spot_bt/behaviors/actions/perception/fiducial.py +++ b/src/spot_bt/behaviors/actions/perception/fiducial.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from bosdyn.api import world_object_pb2 from bosdyn.client.world_object import WorldObjectClient @@ -16,6 +18,10 @@ def __init__(self, name: str): None # google.protobuf.pyext._message.RepeatedCompositeContainer ) + def setup(self, **kwargs): + """Setup DetectFiducialMarkers behavior before initialization.""" + self.logger.debug(f" {self.name} [DetectFiducialMarkers::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [DetectFiducialMarkers::initialise()]") @@ -33,14 +39,10 @@ def initialise(self): def update(self) -> py_trees.common.Status: """Run the TakeImage behavior when ticked.""" self.logger.debug(f" {self.name} [DetectFiducialMarkers::update()]") - try: - request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] - self.fiducials = self.client.list_world_objects( - object_type=request_fiducials - ).world_objects - print(self.fiducials) - except: - return py_trees.common.Status.FAILURE + request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] + self.fiducials = self.client.list_world_objects( + object_type=request_fiducials + ).world_objects return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/perception/graph.py b/src/spot_bt/behaviors/actions/perception/graph.py index 5019989..b704696 100755 --- a/src/spot_bt/behaviors/actions/perception/graph.py +++ b/src/spot_bt/behaviors/actions/perception/graph.py @@ -1,9 +1,10 @@ +from __future__ import annotations + import os from bosdyn.client.graph_nav import GraphNavClient from bosdyn.client.map_processing import MapProcessingServiceClient -from bosdyn.client.recording import GraphNavRecordingServiceClient -from bosdyn.client.recording import NotReadyYetError +from bosdyn.client.recording import GraphNavRecordingServiceClient, NotReadyYetError import py_trees @@ -90,7 +91,7 @@ def update(self) -> py_trees.common.Status: self.logger.debug(f" {status}") return py_trees.common.Status.SUCCESS - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE def terminate(self, new_status: str): @@ -101,7 +102,7 @@ def terminate(self, new_status: str): self.blackboard.graph.map_processing_client = self.map_processing_client self.logger.debug( f" {self.name} [StartRecordingGraph::terminate().terminate()]" - + f"[{self.status}->{new_status}]" + f"[{self.status}->{new_status}]" ) @@ -143,7 +144,7 @@ def update(self) -> py_trees.common.Status: except NotReadyYetError: return py_trees.common.Status.RUNNING - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE def terminate(self, new_status: str): @@ -151,7 +152,7 @@ def terminate(self, new_status: str): self.blackboard.graph.recording_client = self.recording_client self.logger.debug( f" {self.name} [StartRecordingGraph::terminate().terminate()]" - + f"[{self.status}->{new_status}]" + f"[{self.status}->{new_status}]" ) @@ -218,7 +219,7 @@ def terminate(self, new_status: str): self.blackboard.graph.graph_nav_client = self.graph_nav_client self.logger.debug( f" {self.name} [DownloadGraph::terminate().terminate()]" - + f"[{self.status}->{new_status}]" + f"[{self.status}->{new_status}]" ) @@ -264,5 +265,5 @@ def terminate(self, new_status: str): """Terminate behavior and save information.""" self.logger.debug( f" {self.name} [OptimizeAnchoring::terminate().terminate()]" - + f"[{self.status}->{new_status}]" + f"[{self.status}->{new_status}]" ) \ No newline at end of file diff --git a/src/spot_bt/behaviors/actions/perception/image.py b/src/spot_bt/behaviors/actions/perception/image.py index 254ada3..50a93e6 100755 --- a/src/spot_bt/behaviors/actions/perception/image.py +++ b/src/spot_bt/behaviors/actions/perception/image.py @@ -5,8 +5,7 @@ import cv2 from bosdyn.api import image_pb2 -from bosdyn.client.image import build_image_request -from bosdyn.client.image import ImageClient +from bosdyn.client.image import build_image_request, ImageClient import py_trees @@ -15,11 +14,13 @@ from scipy import ndimage from spot_bt.data import Blackboards -from spot_bt.cameras import get_encoding_for_pixel_format_string -from spot_bt.cameras import IMAGE_ROTATION_ANGLES -from spot_bt.cameras import IMAGE_CAMERA_OPTIONS -from spot_bt.cameras import IMAGE_CAMERA_OPTIONS_WITH_ARM -from spot_bt.cameras import pixel_format_string_to_enum +from spot_bt.cameras import ( + get_encoding_for_pixel_format_string, + IMAGE_ROTATION_ANGLES, + IMAGE_CAMERA_OPTIONS, + IMAGE_CAMERA_OPTIONS_WITH_ARM, + pixel_format_string_to_enum, +) class SwitchCamera(py_trees.behaviour.Behaviour): @@ -32,6 +33,10 @@ def __init__(self, name: str, use_arm: bool = False): else: self.image_sources = IMAGE_CAMERA_OPTIONS + def setup(self, **kwargs): + """Setup SwitchCamera behavior before initialization.""" + self.logger.debug(f" {self.name} [SwitchCamera::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [SwitchCamera::initialise()]") @@ -55,7 +60,8 @@ def update(self) -> py_trees.common.Status: self.camera = self.image_sources[idx + 1] return py_trees.common.Status.SUCCESS - except: + + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE @@ -83,6 +89,10 @@ def __init__( self.pixel_format = pixel_format_string_to_enum(pixel_format) self.encoding_data = get_encoding_for_pixel_format_string(pixel_format) + def setup(self, **kwargs): + """Setup TakeImage behavior before initialization.""" + self.logger.debug(f" {self.name} [TakeImage::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [TakeImage::initialise()]") @@ -104,6 +114,7 @@ def initialise(self): def update(self) -> py_trees.common.Status: """Run the TakeImage behavior when ticked.""" + # pylint: disable=no-member self.logger.debug(f" {self.name} [TakeImage::update()]") try: image_request = [ @@ -138,7 +149,7 @@ def update(self) -> py_trees.common.Status: self.image = img cv2.imwrite(f"{os.getcwd()}/test_{self.image_source}{extension}", img) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -171,6 +182,10 @@ def __init__( else: self.image_sources = IMAGE_CAMERA_OPTIONS + def setup(self, **kwargs): + """Setup TakeImageAllCameras behavior before initialization.""" + self.logger.debug(f" {self.name} [TakeImageAllCameras::setup()]") + def initialise(self): """Initialize robot object and client for behavior on first tick.""" self.logger.debug(f" {self.name} [TakeImageAllCameras::initialise()]") @@ -187,6 +202,7 @@ def initialise(self): def update(self) -> py_trees.common.Status: """Run the TakeImageAllCameras behavior when ticked.""" + # pylint: disable=no-member self.logger.debug(f" {self.name} [TakeImageAllCameras::update()]") try: self.images = [] @@ -225,7 +241,7 @@ def update(self) -> py_trees.common.Status: self.images.append(img) cv2.imwrite(f"{os.getcwd()}/test_{source}{extension}", img) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/perception/objects.py b/src/spot_bt/behaviors/actions/perception/objects.py index 57b24b3..88acb01 100755 --- a/src/spot_bt/behaviors/actions/perception/objects.py +++ b/src/spot_bt/behaviors/actions/perception/objects.py @@ -1,4 +1,5 @@ -from bosdyn.api import world_object_pb2 +from __future__ import annotations + from bosdyn.client.world_object import WorldObjectClient import py_trees @@ -50,7 +51,7 @@ def update(self) -> py_trees.common.Status: f"Child frame name: {edge}. Parent frame name: " f"{full_snapshot.child_to_parent_edge_map[edge].parent_frame_name}" ) - except: + except: # pylint: disable=bare-except return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS diff --git a/src/spot_bt/behaviors/actions/template.py b/src/spot_bt/behaviors/actions/template.py index 9bd2fae..bf1b27d 100755 --- a/src/spot_bt/behaviors/actions/template.py +++ b/src/spot_bt/behaviors/actions/template.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import py_trees import random diff --git a/src/spot_bt/behaviors/conditions/general/__init__.py b/src/spot_bt/behaviors/conditions/general/__init__.py index e69de29..854e3b9 100755 --- a/src/spot_bt/behaviors/conditions/general/__init__.py +++ b/src/spot_bt/behaviors/conditions/general/__init__.py @@ -0,0 +1 @@ +from spot_bt.behaviors.conditions.general.docking import * \ No newline at end of file diff --git a/src/spot_bt/behaviors/conditions/general/docking.py b/src/spot_bt/behaviors/conditions/general/docking.py new file mode 100644 index 0000000..b1674eb --- /dev/null +++ b/src/spot_bt/behaviors/conditions/general/docking.py @@ -0,0 +1,37 @@ +"""Spot Docking Related Conditions.""" +from __future__ import annotations + +import py_trees + +from spot_bt.data import Blackboards + + +class IsRobotUndocked(py_trees.behaviour.Behaviour): + def __init__(self, name: str): + super().__init__(name) + self.blackboard = Blackboards() + self.is_docked = False + + def initialise(self): + """Initialize robot and client objects for condition on first tick.""" + self.logger.debug(f" {self.name} [IsRobotUndocked::initialise()]") + self.blackboard.state = self.attach_blackboard_client("State") + self.blackboard.state.register_key( + key="is_docked", access=py_trees.common.Access.READ + ) + self.is_docked = self.blackboard.state.is_docked + + def update(self): + """Run the IsRobotUndocked condition when ticked.""" + self.logger.debug(f" {self.name} [IsRobotUndocked::update()]") + if self.is_docked: + return py_trees.common.Status.FAILURE + + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status: str): + """Terminate condition and save information.""" + self.logger.debug( + f" {self.name} [IsRobotUndocked::terminate().terminate()]" + f"[{self.status}->{new_status}]" + ) diff --git a/src/spot_bt/behaviors/conditions/perception/fiducial.py b/src/spot_bt/behaviors/conditions/perception/fiducial.py index b577776..6fb084f 100755 --- a/src/spot_bt/behaviors/conditions/perception/fiducial.py +++ b/src/spot_bt/behaviors/conditions/perception/fiducial.py @@ -1,4 +1,6 @@ """Fiducial marker related conditions.""" +from __future__ import annotations + import py_trees from spot_bt.data import Blackboards @@ -38,7 +40,7 @@ def update(self) -> py_trees.common.Status: return py_trees.common.Status.FAILURE if not self.include_dock: - if len(self.fiducials) == 1 and int(self.dock_id) == self.fiducials.apriltag_properties.tag_id: + if len(self.fiducials) == 1 and int(self.dock_id) == self.fiducials[0].apriltag_properties.tag_id: return py_trees.common.Status.FAILURE return py_trees.common.Status.SUCCESS @@ -46,7 +48,7 @@ def update(self) -> py_trees.common.Status: def terminate(self, new_status: str): """Terminate condition and save information.""" self.logger.debug( - f" {self.name} [IsAnyFiducialMarkerDetected::terminate().terminate()]" + + f" {self.name} [IsAnyFiducialMarkerDetected::terminate().terminate()]" f"[{self.status}->{new_status}]" ) @@ -89,7 +91,7 @@ def update(self) -> py_trees.common.Status: def terminate(self, new_status: str): """Terminate condition and save information.""" self.logger.debug( - f" {self.name} [IsDockFiducialMarkerDetected::terminate().terminate()]" + + f" {self.name} [IsDockFiducialMarkerDetected::terminate().terminate()]" f"[{self.status}->{new_status}]" ) @@ -131,10 +133,9 @@ def update(self) -> py_trees.common.Status: return py_trees.common.Status.FAILURE - def terminate(self, new_status: str): """Terminate condition and save information.""" self.logger.debug( - f" {self.name} [IsSpecificFiducialMarkerDetected::terminate().terminate()]" + + f" {self.name} [IsSpecificFiducialMarkerDetected::terminate().terminate()]" f"[{self.status}->{new_status}]" ) diff --git a/src/spot_bt/behaviors/conditions/perception/graph.py b/src/spot_bt/behaviors/conditions/perception/graph.py index f9b125f..265f198 100755 --- a/src/spot_bt/behaviors/conditions/perception/graph.py +++ b/src/spot_bt/behaviors/conditions/perception/graph.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from bosdyn.client.recording import GraphNavRecordingServiceClient import py_trees @@ -14,7 +16,7 @@ def __init__(self, name: str): self.robot = None self.recording_client = None - def setup(self): + def setup(self, **kwargs): """Setup the Graph Recording.""" self.logger.debug(f"\t{self.name} [IsGraphRecording::setup()]") diff --git a/src/spot_bt/cameras.py b/src/spot_bt/cameras.py index 2a10686..fc82246 100755 --- a/src/spot_bt/cameras.py +++ b/src/spot_bt/cameras.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from bosdyn.api import image_pb2 import cv2 @@ -7,7 +9,7 @@ import numpy as np -DEPTH_CAMERA_OPTIONS = [ +DEPTH_CAMERA_OPTIONS: list[str] = [ "frontleft_depth", "frontright_depth", "left_depth", @@ -16,7 +18,7 @@ ] -DEPTH_CAMERA_OPTIONS_WITH_ARM = [ +DEPTH_CAMERA_OPTIONS_WITH_ARM: list[str] = [ "frontleft_depth", "frontright_depth", "left_depth", @@ -26,7 +28,7 @@ ] -PIXEL_FORMAT_ENUM_ENCODING_INFORMATION = { +PIXEL_FORMAT_ENUM_ENCODING_INFORMATION: dict[int, tuple[type, int, str]] = { 1: (np.uint8, 1, ".jpg"), 3: (np.uint8, 3, ".jpg"), 4: (np.uint8, 4, ".jpg"), @@ -35,7 +37,7 @@ } -PIXEL_FORMAT_STRING_ENCODING_INFORMATION = { +PIXEL_FORMAT_STRING_ENCODING_INFORMATION: dict[str, tuple[type, int, str]] = { "PIXEL_FORMAT_GREYSCALE_U8": (np.uint8, 1, ".jpg"), "PIXEL_FORMAT_RGB_U8": (np.uint8, 3, ".jpg"), "PIXEL_FORMAT_RGBA_U8": (np.uint8, 4, ".jpg"), @@ -44,7 +46,7 @@ } -IMAGE_CAMERA_OPTIONS = [ +IMAGE_CAMERA_OPTIONS: list[str] = [ "frontleft_fisheye_image", "frontright_fisheye_image", "left_fisheye_image", @@ -53,7 +55,7 @@ ] -IMAGE_CAMERA_OPTIONS_WITH_ARM = [ +IMAGE_CAMERA_OPTIONS_WITH_ARM: list[str] = [ "frontleft_fisheye_image", "frontright_fisheye_image", "left_fisheye_image", @@ -63,7 +65,7 @@ ] -IMAGE_ROTATION_ANGLES = { +IMAGE_ROTATION_ANGLES: dict[str, float | int] = { "back_fisheye_image": 0, "frontleft_fisheye_image": -78, "frontright_fisheye_image": -102, @@ -93,6 +95,8 @@ def find_center_pixel(polygon) -> tuple[int, int]: def get_bounding_box_image(response): + """Calculate and display bounding boxes for world objects in an image.""" + # pylint: disable=no-member dtype = np.uint8 img = np.fromstring(response.image_response.shot.image.data, dtype=dtype) if response.image_response.shot.image.format == image_pb2.Image.FORMAT_RAW: @@ -131,7 +135,7 @@ def get_bounding_box_image(response): def get_encoding_for_pixel_format_string( pixel_format: str, -) -> tuple[type, int, str]: +) -> tuple[np.dtype, int, str]: """Return encoding information for specific pixel format string.""" return PIXEL_FORMAT_STRING_ENCODING_INFORMATION[pixel_format] @@ -143,9 +147,9 @@ def get_encoding_for_pixel_format_enum(pixel_format: int) -> tuple[type, int, st def pixel_format_type_strings() -> list[str]: """Return names of possible image pixel formats.""" - return image_pb2.Image.PixelFormat.keys()[1:] + return image_pb2.Image.PixelFormat.keys()[1:] # pylint: disable=no-member def pixel_format_string_to_enum(enum_string: str) -> int: """Convert pixel format string to respective enum.""" - return dict(image_pb2.Image.PixelFormat.items()).get(enum_string) + return dict(image_pb2.Image.PixelFormat.items()).get(enum_string) # pylint: disable=no-member diff --git a/src/spot_bt/composites/parallel.py b/src/spot_bt/composites/parallel.py index e137480..25409cd 100755 --- a/src/spot_bt/composites/parallel.py +++ b/src/spot_bt/composites/parallel.py @@ -1 +1 @@ -"""Spot Inspecta Parallel composites.""" \ No newline at end of file +"""spot_bt Parallel composites.""" \ No newline at end of file diff --git a/src/spot_bt/composites/selector.py b/src/spot_bt/composites/selector.py index f0cbc72..b3f9ea7 100755 --- a/src/spot_bt/composites/selector.py +++ b/src/spot_bt/composites/selector.py @@ -1,44 +1,34 @@ -"""Spot Inspecta Selector composites.""" -import py_trees +"""spot_bt Selector composites.""" +from __future__ import annotations + +from py_trees.composites import Selector from spot_bt.behaviors.actions.perception import DetectFiducialMarkers -from spot_bt.behaviors.actions.perception import SwitchCamera -from spot_bt.behaviors.actions.perception import TakeImage from spot_bt.behaviors.conditions.perception import IsAnyFiducialMarkerDetected -from spot_bt.behaviors.conditions.perception import IsDockFiducialMarkerDetected -from spot_bt.composites.sequence import create_exploration_sequence +from spot_bt.behaviors.conditions.general import IsRobotUndocked +from spot_bt.composites.sequence import create_undock_sequence -def create_fiducial_search_selector(memory: bool = True) -> py_trees.composites.Selector: - """Create a generic fiducial search selector for Spot.""" - selector = py_trees.composites.Selector("Fiducial Search", memory=memory) +def create_generic_fiducial_selector(memory: bool = True) -> Selector: + """Create a generic fiducial selector for Spot.""" + selector = Selector("Fiducial Search", memory=memory) selector.add_children( [ IsAnyFiducialMarkerDetected(name="Is Fiducial Detected?"), - create_exploration_sequence(), + DetectFiducialMarkers(name="Detect Fiducial Markers"), ] ) return selector -def create_dock_search_selector(memory: bool = True) -> py_trees.composites.Selector: - """Create a generic fiducial search selector for Spot.""" - selector = py_trees.composites.Selector("Fiducial Search", memory=memory) - selector.add_child( - IsDockFiducialMarkerDetected(name="Is Dock Fiducial Detected?") - ) - selector.add_child(create_exploration_sequence()) - return selector - - -def create_generic_fiducial_selector(memory: bool = True) -> py_trees.composites.Selector: - """Create a generic fiducial selector for Spot.""" - selector = py_trees.composites.Selector("Fiducial Search", memory=memory) +def create_undock_selector(memory: bool = True) -> Selector: + """Create an undock selector with a dock state check.""" + selector = Selector("Check and Undock", memory) selector.add_children( [ - IsAnyFiducialMarkerDetected(name="Is Fiducial Detected?"), - DetectFiducialMarkers(name="Detect Fiducial Markers"), + IsRobotUndocked("Is Robot Undocked?"), + create_undock_sequence(memory=memory), ] ) diff --git a/src/spot_bt/composites/sequence.py b/src/spot_bt/composites/sequence.py index abf3827..fb0be52 100755 --- a/src/spot_bt/composites/sequence.py +++ b/src/spot_bt/composites/sequence.py @@ -1,19 +1,22 @@ -"""Spot Inspecta Sequence composites.""" -import py_trees - -from spot_bt.behaviors.actions.general import RobotDock -from spot_bt.behaviors.actions.general import RobotUndock -from spot_bt.behaviors.actions.general import RobotPowerOff -from spot_bt.behaviors.actions.general import RobotPowerOn -from spot_bt.behaviors.actions.general import RobotState +"""spot_bt Sequence composites.""" +from __future__ import annotations + +from py_trees.composites import Sequence + +from spot_bt.behaviors.actions.general import ( + RobotDock, + RobotUndock, + RobotPowerOff, + RobotPowerOn, + RobotState, +) from spot_bt.behaviors.actions.perception import DetectFiducialMarkers -from spot_bt.behaviors.actions.perception import TakeImage from spot_bt.behaviors.conditions.perception import IsAnyFiducialMarkerDetected -def create_dock_sequence(name: str = "Dock and Turn Off", memory: bool = True) -> py_trees.composites.Sequence: +def create_dock_sequence(name: str = "Dock and Turn Off", memory: bool = True) -> Sequence: """Create a generic Dock Sequence for Spot.""" - sequence = py_trees.composites.Sequence(name, memory=memory) + sequence = Sequence(name, memory=memory) sequence.add_children( [ RobotState(name="Get Spot State"), @@ -25,49 +28,9 @@ def create_dock_sequence(name: str = "Dock and Turn Off", memory: bool = True) - return sequence -def create_exploration_sequence(name: str = "Explore", memory: bool = True) -> py_trees.composites.Sequence: - """Create an exploration sequence for Spot.""" - sequence = py_trees.composites.Sequence(name, memory=memory) - sequence.add_children( - [ - RobotState(name="Get Spot State"), - DetectFiducialMarkers(name="Detect Fiducial Markers"), - IsAnyFiducialMarkerDetected(name="Is Fiducial Detected?"), - ] - ) - - return sequence - - -def create_find_dock_sequence(name: str = "Find Dock", memory: bool = True) -> py_trees.composites.Sequence: - """Create a dock finding sequence for Spot.""" - sequence = py_trees.composites.Sequence(name, memory=memory) - sequence.add_children([ - RobotState(name="Get Spot State"), - DetectFiducialMarkers(name="Detect") - ] - ) - - return sequence - - -def create_move_to_target_sequence(name: str = "Explore", memory: bool = True) -> py_trees.composites.Sequence: - """Create an exploration sequence for Spot.""" - sequence = py_trees.composites.Sequence(name, memory=memory) - sequence.add_children( - [ - RobotState(name="Get Spot State"), - DetectFiducialMarkers(name="Detect Fiducial Markers"), - IsAnyFiducialMarkerDetected(name="Is Fiducial Detected?"), - ] - ) - - return sequence - - -def create_undock_sequence(name: str = "Turn On and Undock", memory: bool = True) -> py_trees.composites.Sequence: +def create_undock_sequence(name: str = "Turn On and Undock", memory: bool = True) -> Sequence: """Create a generic Undock Sequence for Spot.""" - sequence = py_trees.composites.Sequence(name, memory=memory) + sequence = Sequence(name, memory=memory) sequence.add_children( [ RobotPowerOn(name="Power ON Spot"), diff --git a/src/spot_bt/data.py b/src/spot_bt/data.py index 4d283c6..41e40b5 100644 --- a/src/spot_bt/data.py +++ b/src/spot_bt/data.py @@ -1,19 +1,17 @@ -from dataclasses import dataclass +from __future__ import annotations -from bosdyn.api import arm_command_pb2 -from bosdyn.api import geometry_pb2 -from bosdyn.api import synchronized_command_pb2 -from bosdyn.api import trajectory_pb2 +from dataclasses import dataclass, field + +from bosdyn.api import ( + arm_command_pb2, + geometry_pb2, + synchronized_command_pb2, + trajectory_pb2, +) from bosdyn.api.spot import robot_command_pb2 -from bosdyn.client.frame_helpers import get_a_tform_b -from bosdyn.client.frame_helpers import get_se2_a_tform_b -from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME -from bosdyn.client.frame_helpers import ODOM_FRAME_NAME -from bosdyn.client.frame_helpers import VISION_FRAME_NAME -from bosdyn.client.math_helpers import Quat -from bosdyn.client.math_helpers import SE2Pose -from bosdyn.client.math_helpers import SE3Pose -from bosdyn.client.math_helpers import Vec3 +from bosdyn.client.frame_helpers import get_a_tform_b, get_se2_a_tform_b +from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME, VISION_FRAME_NAME +from bosdyn.client.math_helpers import Quat, SE2Pose, SE3Pose, Vec3 from bosdyn.client.robot_command import RobotCommandBuilder from bosdyn.geometry import EulerZXY from bosdyn.util import seconds_to_duration @@ -27,24 +25,28 @@ class Blackboards: """Dataclass for various BT Blackboards related to spot_inspecta.""" - state: py_trees.blackboard.Client | None = None - arm: py_trees.blackboard.Client | None = None - graph: py_trees.blackboard.Client | None = None - perception: py_trees.blackboard.Client | None = None - ins: py_trees.blackboard.Client | None = None + state: py_trees.blackboard.Client | None = field(default=None) + arm: py_trees.blackboard.Client | None = field(default=None) + graph: py_trees.blackboard.Client | None = field(default=None) + perception: py_trees.blackboard.Client | None = field(default=None) + ins: py_trees.blackboard.Client | None = field(default=None) @dataclass class Pose: """Dataclass for pose in free space, composed of position and orientation.""" - position: geometry_pb2.Vec3 = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) - orientation: geometry_pb2.Quaternion = geometry_pb2.Quaternion( - w=0.0, x=0.0, y=0.0, z=0.0 + position: geometry_pb2.Vec3 = field( + default_factory=lambda: geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) + ) + orientation: geometry_pb2.Quaternion = field( + default_factory=lambda: geometry_pb2.Quaternion( + w=0.0, x=0.0, y=0.0, z=0.0 + ) ) - pose: EulerZXY = EulerZXY(yaw=0.0, roll=0.0, pitch=0.0) - body_height: float = 0.0 - is_done: bool = False + pose: EulerZXY = field(default_factory=lambda: EulerZXY(yaw=0.0, roll=0.0, pitch=0.0)) + body_height: float = field(default=0.0) + is_done: bool = field(default=False) def as_list(self) -> list[float | int]: """Get values as a Python List.""" @@ -112,10 +114,10 @@ def create_command(self) -> RobotCommandBuilder: class Movement: """Dataclass for movement in free space.""" - v_x: float = 0.0 - v_y: float = 0.0 - v_rot: float = 0.0 - is_done: bool = False + v_x: float = field(default=0.0) + v_y: float = field(default=0.0) + v_rot: float = field(default=0.0) + is_done: bool = field(default=False) def as_list(self) -> list[float | int]: """Get values as a Python List.""" @@ -151,7 +153,9 @@ def create_command(self) -> RobotCommandBuilder: class BodyTrajectory: """Dataclass for a Spot movement trajectory.""" - pose: SE2Pose | SE3Pose = SE3Pose(x=0.0, y=0.0, z=0.0, rot=0.0) + pose: SE2Pose | SE3Pose = field( + default_factory=lambda: SE3Pose(x=0.0, y=0.0, z=0.0, rot=0.0) + ) def mark(self): """Set that the pose stored has been completed.""" @@ -175,8 +179,10 @@ def create_rotation_trajectory_command( """Create an in-place rotation trajectory to pass to the client.""" # if isinstance(self.pose, SE3Pose): # raise TypeError("Rotations require use of SE2Pose not SE3Pose") - if frame_type in ["odom", "vision"]: - frame = ODOM_FRAME_NAME if frame_type == "odom" else VISION_FRAME_NAME + if frame_type not in ["odom", "vision"]: + frame = VISION_FRAME_NAME + else: + frame = frame_type body_tform_goal = SE2Pose(x=0.0, y=0.0, angle=angle) frame_tform_body = get_se2_a_tform_b( @@ -230,8 +236,8 @@ def create_full_trajectory_command( class ArmPose: """Dataclass for a single Spot Arm Pose.""" - pose: SE3Pose = SE3Pose(x=0.0, y=0.0, z=0.0, rot=Quat()) - is_done: bool = False + pose: SE3Pose = field(default_factory=lambda: SE3Pose(x=0.0, y=0.0, z=0.0, rot=Quat())) + is_done: bool = field(default=False) def hand_pose(self) -> geometry_pb2.Vec3: """Get the hand pose relative to target.""" @@ -250,8 +256,10 @@ def hand_orientation(self) -> geometry_pb2.Quaternion: class ArmPoses: """Dataclass for multiple poses that constitute a Spot/Arm trajectory.""" - poses: list[SE3Pose | ArmPose] = SE3Pose(x=0.0, y=0.0, z=0.0, rot=Quat()) - is_done: bool = False + poses: list[SE3Pose | ArmPose] = field( + default_factory=lambda: SE3Pose(x=0.0, y=0.0, z=0.0, rot=Quat()) + ) + is_done: bool = field(default=False) def mark(self): """Set that the pose stored has been completed.""" @@ -281,6 +289,7 @@ def create_command( self, seconds: int = 2, gripper_open_fraction: float = 0.0 ) -> RobotCommandBuilder: """Create an arm trajectory command to pass to client.""" + # pylint: disable=no-member trajectories = [] for pose in self.poses: trajectories.append( diff --git a/src/spot_bt/tick.py b/src/spot_bt/tick.py index d88f9ea..3286071 100755 --- a/src/spot_bt/tick.py +++ b/src/spot_bt/tick.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import py_trees diff --git a/src/spot_bt/utils.py b/src/spot_bt/utils.py index 4521444..e80e355 100755 --- a/src/spot_bt/utils.py +++ b/src/spot_bt/utils.py @@ -1,6 +1,7 @@ +from __future__ import annotations + from bosdyn import geometry -from bosdyn.api import geometry_pb2 -from bosdyn.api import trajectory_pb2 +from bosdyn.api import geometry_pb2, trajectory_pb2 from bosdyn.api.spot import robot_command_pb2 from bosdyn.client.math_helpers import Quat @@ -20,7 +21,7 @@ def get_default_mobility_parameters( y_velocity_limit: float = 0.5, angular_velocity_limit: float = 1.0, ) -> robot_command_pb2.MobilityParams: - """Something.""" + """Generate default mobility parameters for Spot motion.""" obstacles = robot_command_pb2.ObstacleParams( disable_vision_body_obstacle_avoidance=True, disable_vision_foot_obstacle_avoidance=True,