Skip to content

Commit

Permalink
[SW-1788] use synchros2 in spot_ros2 (#549)
Browse files Browse the repository at this point in the history
## Change Overview

Mass replaces `bdai_ros2_wrappers` with `synchros2` so that there is no extra aliasing (which should make all imports faster, hopefully helping internal CI tests?)

## Testing Done

- [x] if CI passes, this is okay
- [x] run driver on robot for sanity check

Signed-off-by: Katie Hughes <[email protected]>
  • Loading branch information
khughes-bdai authored and Katie Hughes committed Jan 14, 2025
1 parent eba2b17 commit 1d46cf3
Show file tree
Hide file tree
Showing 35 changed files with 86 additions and 90 deletions.
2 changes: 1 addition & 1 deletion spot_driver/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
<depend>tf2_ros</depend>
<depend>tl_expected</depend>

<exec_depend>bdai_ros2_wrappers</exec_depend>
<exec_depend>synchros2</exec_depend>
<exec_depend>python3-protobuf</exec_depend>
<exec_depend>python3-tk</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,15 +18,15 @@
import argparse
from typing import Optional

import bdai_ros2_wrappers.process as ros_process
import bdai_ros2_wrappers.scope as ros_scope
import cv2
import numpy as np
import open3d as o3d
import synchros2.process as ros_process
import synchros2.scope as ros_scope
import yaml
from bdai_ros2_wrappers.context import wait_for_shutdown
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from synchros2.context import wait_for_shutdown


class CalibratedReRegisteredHandCameraDepthPublisher:
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/spot_driver/spot_alerts.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@
from tkinter import messagebox
from typing import Any, List, Optional

import bdai_ros2_wrappers.process as ros_process
from bdai_ros2_wrappers.node import Node
import synchros2.process as ros_process
from rclpy.parameter import Parameter
from rclpy.qos import QoSPresetProfiles
from synchros2.node import Node

from spot_msgs.msg import ( # type: ignore
BatteryStateArray,
Expand Down
12 changes: 4 additions & 8 deletions spot_driver/spot_driver/spot_ros2.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,12 @@
from enum import Enum
from typing import Any, Callable, Dict, List, Optional, Union

import bdai_ros2_wrappers.process as ros_process
import builtin_interfaces.msg
import rclpy
import rclpy.duration
import rclpy.time
import synchros2.process as ros_process
import tf2_ros
from bdai_ros2_wrappers.node import Node
from bdai_ros2_wrappers.single_goal_action_server import (
SingleGoalActionServer,
)
from bdai_ros2_wrappers.single_goal_multiple_action_servers import (
SingleGoalMultipleActionServers,
)
from bosdyn.api import (
geometry_pb2,
gripper_camera_param_pb2,
Expand Down Expand Up @@ -69,6 +62,9 @@
from rclpy.timer import Rate
from sensor_msgs.msg import JointState
from std_srvs.srv import SetBool, Trigger
from synchros2.node import Node
from synchros2.single_goal_action_server import SingleGoalActionServer
from synchros2.single_goal_multiple_action_servers import SingleGoalMultipleActionServers

import spot_driver.robot_command_util as robot_command_util

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/conftest.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
import tempfile
import typing

import bdai_ros2_wrappers.scope as ros_scope
import domain_coordinator
import grpc
import launch
Expand All @@ -31,11 +30,12 @@
import launch_ros.substitutions
import pytest
import rclpy
import synchros2.scope as ros_scope
import yaml
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.power_pb2 import PowerCommandRequest, PowerCommandResponse, PowerCommandStatus
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from bosdyn.api.robot_state_pb2 import PowerState
from synchros2.scope import ROSAwareScope

import spot_wrapper.testing
from spot_driver.spot_ros2 import SpotROS
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_clear_behavior_fault.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import ClearBehaviorFaultResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import ClearBehaviorFault # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_dock.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,11 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.basic_command_pb2 import RobotCommandFeedbackStatus, StandCommand
from bosdyn.api.docking.docking_pb2 import DockingCommandFeedbackResponse, DockingCommandResponse
from bosdyn.api.robot_command_pb2 import RobotCommandFeedbackResponse, RobotCommandResponse
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import Dock # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_execute_dance.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,6 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.spot.choreography_sequence_pb2 import (
ChoreographySequence,
ChoreographyStatusResponse,
Expand All @@ -20,6 +18,8 @@
from google.protobuf import text_format
from rclpy.action import ActionClient
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.action import ExecuteDance # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_get_choreography_status.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.spot.choreography_sequence_pb2 import ChoreographyStatusResponse
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import GetChoreographyStatus # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_list_all_dances.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.spot.choreography_sequence_pb2 import ListAllSequencesResponse
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import ListAllDances # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_list_all_moves.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.spot.choreography_sequence_pb2 import ListAllMovesResponse
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import ListAllMoves # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_locomotion_mode.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import SetLocomotion # type: ignore

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_max_velocity.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import SetVelocity # type: ignore

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_power_off.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_recorded_state_to_animation.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,6 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.data_chunk_pb2 import DataChunk
from bosdyn.api.geometry_pb2 import Quaternion, SE3Pose, Vec3
from bosdyn.api.header_pb2 import CommonError, ResponseHeader
Expand All @@ -25,6 +23,8 @@
)
from google.protobuf.timestamp_pb2 import Timestamp
from google.protobuf.wrappers_pb2 import DoubleValue
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import ChoreographyRecordedStateToAnimation # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_release.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope


@pytest.mark.usefixtures("spot_node", "simple_spot")
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_robot_command.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,14 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.basic_command_pb2 import RobotCommandFeedbackStatus, StopCommand
from bosdyn.api.full_body_command_pb2 import FullBodyCommand
from bosdyn.api.robot_command_pb2 import RobotCommandFeedback, RobotCommandFeedbackResponse, RobotCommandResponse
from bosdyn.client.robot_command import RobotCommandBuilder
from bosdyn_msgs.conversions import convert
from rclpy.action import ActionClient
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.action import RobotCommand as RobotCommandAction # type: ignore
from spot_msgs.srv import RobotCommand as RobotCommandService # type: ignore
Expand Down
8 changes: 4 additions & 4 deletions spot_driver/test/pytests/test_robot_state.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,11 @@
import typing

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bdai_ros2_wrappers.subscription import Subscription
from bdai_ros2_wrappers.utilities import namespace_with
from sensor_msgs.msg import JointState
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope
from synchros2.subscription import Subscription
from synchros2.utilities import namespace_with

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_rollover.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
2 changes: 1 addition & 1 deletion spot_driver/test/pytests/test_ros_interfaces.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
import contextlib
import unittest

import bdai_ros2_wrappers.scope as ros_scope
import rclpy
import synchros2.scope as ros_scope
from bosdyn_msgs.msg import RobotCommand, RobotCommandFeedback
from std_srvs.srv import Trigger

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_self_right.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_sit.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_stair_mode.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from std_srvs.srv import SetBool
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope


@pytest.mark.usefixtures("spot_node")
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_stand.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_start_recording_state.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.spot.choreography_sequence_pb2 import StartRecordingStateResponse
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import ChoreographyStartRecordingState # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_stop.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.robot_command_pb2 import RobotCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_stop_recording_state.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.spot.choreography_sequence_pb2 import StopRecordingStateResponse
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_msgs.srv import ChoreographyStopRecordingState # type: ignore
from spot_wrapper.testing.fixtures import SpotFixture
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/test/pytests/test_undock.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
# pylint: disable=no-member

import pytest
from bdai_ros2_wrappers.futures import wait_for_future
from bdai_ros2_wrappers.scope import ROSAwareScope
from bosdyn.api.docking.docking_pb2 import DockingCommandFeedbackResponse, DockingCommandResponse
from std_srvs.srv import Trigger
from synchros2.futures import wait_for_future
from synchros2.scope import ROSAwareScope

from spot_wrapper.testing.fixtures import SpotFixture

Expand Down
Loading

0 comments on commit 1d46cf3

Please sign in to comment.