diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 124702eb..4febb6b5 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -3,7 +3,7 @@ import logging import os from enum import Enum -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Literal, Optional, Tuple import yaml from launch.actions import DeclareLaunchArgument @@ -13,6 +13,16 @@ COLOR_END = "\33[0m" COLOR_YELLOW = "\33[33m" +# The following constants are parameters we are interested in pulling from our config yaml file +_USERNAME: Literal["username"] = "username" +_PASSWORD: Literal["password"] = "password" +_HOSTNAME: Literal["hostname"] = "hostname" +_CERTIFICATE: Literal["certificate"] = "certificate" +_PORT: Literal["port"] = "port" +_CAMERAS_USED: Literal["cameras_used"] = "cameras_used" +_GRIPPERLESS: Literal["gripperless"] = "gripperless" + + IMAGE_PUBLISHER_ARGS = [ "depth_registered_mode", "publish_point_clouds", @@ -151,16 +161,16 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional ros_params = get_ros_param_dict(config_file_path) # only set username/password/hostname if they were not already set as environment variables. - if not username and "username" in ros_params: - username = ros_params["username"] - if not password and "password" in ros_params: - password = ros_params["password"] - if not hostname and "hostname" in ros_params: - hostname = ros_params["hostname"] - if not port and "port" in ros_params: - port = ros_params["port"] - if not certificate and "certificate" in ros_params: - certificate = ros_params["certificate"] + if not username and _USERNAME in ros_params: + username = ros_params[_USERNAME] + if not password and _PASSWORD in ros_params: + password = ros_params[_PASSWORD] + if not hostname and _HOSTNAME in ros_params: + hostname = ros_params[_HOSTNAME] + if not port and _PORT in ros_params: + port = ros_params[_PORT] + if not certificate and _CERTIFICATE in ros_params: + certificate = ros_params[_CERTIFICATE] if (not username) or (not password) or (not hostname): raise ValueError( "One or more of your login credentials has not been specified! Got invalid values of " @@ -177,6 +187,15 @@ def default_camera_sources(has_arm: bool, gripperless: bool) -> List[str]: return camera_sources +def get_gripperless(ros_params: Dict[str, Any]) -> bool: + """Read the ros parameters to get the value of the gripperless parameter. Defaults to False if not set.""" + gripperless = False + if _GRIPPERLESS in ros_params: + if isinstance(ros_params[_GRIPPERLESS], bool): + gripperless = ros_params[_GRIPPERLESS] + return gripperless + + def get_camera_sources_from_ros_params(ros_params: Dict[str, Any], has_arm: bool) -> List[str]: """Get the list of cameras to stream from. This will be taken from the parameters in the config yaml if it exists and contains valid cameras. If this list contains invalid cameras, fall back to the default of all cameras. @@ -191,13 +210,10 @@ def get_camera_sources_from_ros_params(ros_params: Dict[str, Any], has_arm: bool Returns: List[str]: List of cameras the driver will stream from. """ - gripperless = False - if "gripperless" in ros_params: - if isinstance(ros_params["gripperless"], bool): - gripperless = ros_params["gripperless"] + gripperless = get_gripperless(ros_params) default_sources = default_camera_sources(has_arm, gripperless) - if "cameras_used" in ros_params: - camera_sources = ros_params["cameras_used"] + if _CAMERAS_USED in ros_params: + camera_sources = ros_params[_CAMERAS_USED] if isinstance(camera_sources, List): # check if the user inputted any camera that's not in the default sources. invalid_cameras = [cam for cam in camera_sources if cam not in default_sources] @@ -235,6 +251,7 @@ def spot_has_arm(config_file_path: str, spot_name: str) -> bool: """ logger = logging.getLogger("spot_driver_launch") username, password, hostname, port, certificate = get_login_parameters(config_file_path) + gripperless = get_gripperless(get_ros_param_dict(config_file_path)) spot_wrapper = SpotWrapper( username=username, password=password, @@ -243,5 +260,6 @@ def spot_has_arm(config_file_path: str, spot_name: str) -> bool: cert_resource_glob=certificate, robot_name=spot_name, logger=logger, + gripperless=gripperless, ) return spot_wrapper.has_arm() diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 09b2d9cd..3d486f8b 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -405,6 +405,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw continually_try_stand=self.continually_try_stand.value, rgb_cameras=self.rgb_cameras.value, cert_resource_glob=self.certificate, + gripperless=self.gripperless, ) if not self.spot_wrapper.is_valid: return