Skip to content

Commit

Permalink
Fix tests now that Mock_spot is not a sentinel
Browse files Browse the repository at this point in the history
  • Loading branch information
ggoretkin-bdai committed Dec 5, 2023
1 parent 80fd72b commit 9725e97
Show file tree
Hide file tree
Showing 2 changed files with 19 additions and 2 deletions.
11 changes: 11 additions & 0 deletions spot_driver/spot_driver/spot_ros2.py
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,14 @@ class SpotImageType(str, Enum):
RegDepth = "depth_registered"


def set_node_parameter_from_parameter_list(
node: Node, parameter_list: Optional[typing.List[Parameter]], parameter_name: str
) -> None:
"""Set parameters when the node starts not from a launch file."""
if parameter_list is not None:
node.set_parameters([parameter for parameter in parameter_list if parameter.name == parameter_name])


class SpotROS(Node):
"""Parent class for using the wrapper. Defines all callbacks and keeps the wrapper alive"""

Expand Down Expand Up @@ -252,6 +260,9 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw

self.declare_parameter("spot_name", "")
self.declare_parameter("mock_enable", False)

# If `mock_enable:=True`, then there are additional parameters. We must set this one separately.
set_node_parameter_from_parameter_list(self, parameter_list, "mock_enable")
if self.get_parameter("mock_enable").value:
self.declare_parameter("mock_has_arm", rclpy.Parameter.Type.BOOL)

Expand Down
10 changes: 8 additions & 2 deletions spot_driver/test/test_spot_driver.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,14 @@ def setUp(self) -> None:
self.fixture = contextlib.ExitStack()
self.ros = self.fixture.enter_context(ros_scope.top(namespace="fixture"))
# create and run spot ros2 servers
mock_param = rclpy.parameter.Parameter("spot_name", rclpy.Parameter.Type.STRING, "Mock_spot")
self.spot_ros2 = self.ros.load(spot_driver.spot_ros2.SpotROS, parameter_list=[mock_param])
self.spot_ros2 = self.ros.load(
spot_driver.spot_ros2.SpotROS,
parameter_list=[
rclpy.parameter.Parameter("spot_name", value="Mock_spot"),
rclpy.parameter.Parameter("mock_enable", value=True),
rclpy.parameter.Parameter("mock_has_arm", value=False),
],
)

# clients
self.claim_client: rclpy.node.Client = self.ros.node.create_client(Trigger, "claim")
Expand Down

0 comments on commit 9725e97

Please sign in to comment.