Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update breaking changes in spot_wrapper refactoring #41

Closed
wants to merge 9 commits into from
+25 −18
Closed

Update breaking changes in spot_wrapper refactoring #41

Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
76cffb9
update spot_wrapper calls with breaking changes
jeremysee2 Apr 20, 2023
6551826
Merge branch 'main' of https://github.com/bdaiinstitute/spot_ros2
jeremysee2 Apr 20, 2023
48c15bf
Merge branch 'main' of https://github.com/bdaiinstitute/spot_ros2 int…
jeremysee2 Jun 9, 2023
25cec99
Merge branch 'main' of https://github.com/bdaiinstitute/spot_ros2 int…
jeremysee2 Jun 9, 2023
ad235fa
black formatting
jeremysee2 Jun 9, 2023
e5a6437
Merge branch 'main' into main
amessing-bdai Jun 9, 2023
f2a51dc
ruff
amessing-bdai Jun 9, 2023
370ba05
Fixed bug with self.node -> self
amessing-bdai Jun 9, 2023
8015b4e
Merge branch 'main' into main
amessing-bdai Jun 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 25 additions & 18 deletions spot_driver/spot_driver/spot_ros2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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:
Expand All @@ -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:
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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:
Expand All @@ -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"
Expand All @@ -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"
Expand All @@ -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
Expand Down Expand Up @@ -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

Expand All @@ -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()
Expand All @@ -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()

Expand Down