diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 384047cfa..afa88acf6 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -75,7 +75,8 @@ SetLocomotion, SetVelocity, ) -from spot_wrapper.wrapper import CameraSource, SpotWrapper +from spot_wrapper.spot_images import CameraSource +from spot_wrapper.wrapper import SpotWrapper #####DEBUG/RELEASE: RELATIVE PATH NOT WORKING IN DEBUG # Release @@ -796,7 +797,7 @@ def publish_camera_images_callback(self) -> None: if self.spot_wrapper is None: return - result = self.spot_wrapper.get_images_by_cameras( + result = self.spot_wrapper.spot_images.get_images_by_cameras( [CameraSource(camera_name, ["visual"]) for camera_name in self.cameras_used.value] ) for image_entry in result: @@ -813,7 +814,7 @@ def publish_depth_images_callback(self) -> None: if self.spot_wrapper is None: return - result = self.spot_wrapper.get_images_by_cameras( + result = self.spot_wrapper.spot_images.get_images_by_cameras( [CameraSource(camera_name, ["depth"]) for camera_name in self.cameras_used.value] ) for image_entry in result: @@ -830,7 +831,7 @@ def publish_depth_registered_images_callback(self) -> None: if self.spot_wrapper is None: return - result = self.spot_wrapper.get_images_by_cameras( + result = self.spot_wrapper.spot_images.get_images_by_cameras( [CameraSource(camera_name, ["depth_registered"]) for camera_name in self.cameras_used.value] ) for image_entry in result: @@ -1418,7 +1419,7 @@ def _get_manipulation_command_feedback(self, goal_id: str) -> ManipulationApiFee feedback = ManipulationApiFeedbackResponse() if self.spot_wrapper is not None: conv.convert_proto_to_bosdyn_msgs_manipulation_api_feedback_response( - self.spot_wrapper.get_manipulation_command_feedback(goal_id), feedback + self.spot_wrapper.spot_arm.get_manipulation_command_feedback(goal_id), feedback ) return feedback @@ -1679,11 +1680,11 @@ def handle_graph_nav_set_localization( try: if request.method == "fiducial": - self.spot_wrapper._set_initial_localization_fiducial() + self.spot_wrapper.spot_graph_nav.set_initial_localization_fiducial() response.success = True response.message = "Success" elif request.method == "waypoint": - self.spot_wrapper._set_initial_localization_waypoint([request.waypoint_id]) + self.spot_wrapper.spot_graph_nav.set_initial_localization_waypoint([request.waypoint_id]) response.success = True response.message = "Success" else: @@ -1709,7 +1710,7 @@ def handle_graph_nav_upload_graph( try: self.get_logger().info(f"Uploading GraphNav map: {request.upload_filepath}") - self.spot_wrapper._upload_graph_and_snapshots(request.upload_filepath) + self.spot_wrapper.spot_graph_nav.upload_graph_and_snapshots(request.upload_filepath) self.get_logger().info("Uploaded") response.success = True response.message = "Success" @@ -1730,7 +1731,7 @@ def handle_graph_nav_clear_graph( try: self.get_logger().info("Clearing graph") - self.spot_wrapper._clear_graph() + self.spot_wrapper.spot_graph_nav.clear_graph() self.get_logger().info("Cleared") response.success = True response.message = "Success" @@ -1749,11 +1750,11 @@ def handle_list_graph(self, request: ListGraph.Request, response: ListGraph.Resp return response try: - self.get_logger().error(f"handle_list_graph: {request}") - self.spot_wrapper._clear_graph() - self.spot_wrapper._upload_graph_and_snapshots(request.upload_filepath) - response.waypoint_ids = self.spot_wrapper.list_graph(request.upload_filepath) - self.get_logger().error(f"handle_list_graph RESPONSE: {response}") + self.get_logger().error("handle_list_graph: {request}") + self.spot_wrapper.spot_graph_nav.clear_graph() + self.spot_wrapper.spot_graph_nav.upload_graph_and_snapshots(request.upload_filepath) + response.waypoint_ids = self.spot_wrapper.spot_graph_nav.list_graph(request.upload_filepath) + self.get_logger().error("handle_list_graph RESPONSE: {response}") except Exception as e: self.get_logger().error("Exception Error:{}".format(e)) return response @@ -1787,7 +1788,7 @@ def _handle_list_world_objects( world_object.apriltag_properties.frame_name_fiducial = "fiducial_3" world_object.apriltag_properties.frame_name_fiducial_filtered = "filtered_fiducial_3" else: - proto_response = self.spot_wrapper.list_world_objects(object_types, time_start_point) + proto_response = self.spot_wrapper.spot_world_objects.list_world_objects(object_types, time_start_point) conv.convert_proto_to_bosdyn_msgs_list_world_object_response(proto_response, response.response) return response @@ -1814,6 +1815,7 @@ def handle_navigate_to(self, goal_handle: ServerGoalHandle) -> NavigateTo.Result feedback_thread = threading.Thread(target=self.handle_navigate_to_feedback, args=()) self.run_navigate_to = True feedback_thread.start() + if self.spot_wrapper is None: self.get_logger().error("Spot wrapper is None") response = NavigateTo.Result() @@ -1822,13 +1824,18 @@ def handle_navigate_to(self, goal_handle: ServerGoalHandle) -> NavigateTo.Result goal_handle.abort() return response - # run navigate_to - resp = self.spot_wrapper.navigate_to( + # Initialize with fiducial + resp = self.spot_wrapper.spot_graph_nav.navigate_initial_localization( upload_path=goal_handle.request.upload_path, - navigate_to=goal_handle.request.navigate_to, initial_localization_fiducial=goal_handle.request.initial_localization_fiducial, initial_localization_waypoint=goal_handle.request.initial_localization_waypoint, ) + + # Navigate to selected waypoint + resp = self.spot_wrapper.spot_graph_nav.navigate_to_existing_waypoint( + waypoint_id=goal_handle.request.navigate_to + ) + self.run_navigate_to = False feedback_thread.join()