mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-04-20 11:51:16 -05:00
finish adding parameters
This commit is contained in:
@@ -51,6 +51,8 @@ class Anchor(Node):
|
||||
|
||||
logger = self.get_logger()
|
||||
|
||||
# ROS2 Parameter Setup
|
||||
|
||||
self.declare_parameter(
|
||||
"connector",
|
||||
"auto",
|
||||
@@ -73,6 +75,17 @@ class Anchor(Node):
|
||||
),
|
||||
)
|
||||
|
||||
self.declare_parameter(
|
||||
"serial_override",
|
||||
"",
|
||||
ParameterDescriptor(
|
||||
name="serial_override",
|
||||
description="Overrides which serial port will be used. Defaults to ''.",
|
||||
type=ParameterType.PARAMETER_STRING,
|
||||
additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
|
||||
),
|
||||
)
|
||||
|
||||
# Determine which connector to use. Options are Mock, Serial, and CAN
|
||||
connector_select = (
|
||||
self.get_parameter("connector").get_parameter_value().string_value
|
||||
@@ -80,10 +93,15 @@ class Anchor(Node):
|
||||
can_override = (
|
||||
self.get_parameter("can_override").get_parameter_value().string_value
|
||||
)
|
||||
serial_override = (
|
||||
self.get_parameter("serial_override").get_parameter_value().string_value
|
||||
)
|
||||
match connector_select:
|
||||
case "serial":
|
||||
logger.info("using serial connector")
|
||||
self.connector = SerialConnector(logger, self.get_clock())
|
||||
self.connector = SerialConnector(
|
||||
logger, self.get_clock(), serial_override
|
||||
)
|
||||
case "can":
|
||||
logger.info("using CAN connector")
|
||||
self.connector = CANConnector(logger, self.get_clock(), can_override)
|
||||
@@ -99,7 +117,9 @@ class Anchor(Node):
|
||||
)
|
||||
except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
|
||||
logger.info("CAN connector failed, trying serial connector")
|
||||
self.connector = SerialConnector(logger, self.get_clock())
|
||||
self.connector = SerialConnector(
|
||||
logger, self.get_clock(), serial_override
|
||||
)
|
||||
case _:
|
||||
logger.fatal(
|
||||
f"invalid value for connector parameter: {connector_select}"
|
||||
|
||||
@@ -78,12 +78,19 @@ class SerialConnector(Connector):
|
||||
port: str
|
||||
mcu_name: str
|
||||
serial_interface: serial.Serial
|
||||
override: bool
|
||||
|
||||
def __init__(self, logger: RcutilsLogger, clock: Clock):
|
||||
def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
|
||||
super().__init__(logger, clock)
|
||||
|
||||
ports = self._find_ports()
|
||||
mcu_name: str | None = None
|
||||
|
||||
if serial_override:
|
||||
logger.warn(
|
||||
f"using serial_override: `{serial_override}`! this will bypass several checks."
|
||||
)
|
||||
ports = [serial_override]
|
||||
mcu_name = "override"
|
||||
|
||||
if len(ports) <= 0:
|
||||
raise NoValidDeviceException("no valid serial device found")
|
||||
@@ -94,7 +101,8 @@ class SerialConnector(Connector):
|
||||
|
||||
# check each of our ports to make sure one of them is responding
|
||||
port = ports[0]
|
||||
mcu_name = self._get_name(port)
|
||||
# we might already have a name by now if we overrode earlier
|
||||
mcu_name = mcu_name or self._get_name(port)
|
||||
if not mcu_name:
|
||||
raise NoWorkingDeviceException(
|
||||
f"found {port}, but it did not respond with its name"
|
||||
|
||||
@@ -7,6 +7,8 @@ from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
connector = LaunchConfiguration("connector")
|
||||
serial_override = LaunchConfiguration("serial_override")
|
||||
can_override = LaunchConfiguration("can_override")
|
||||
use_ptz = LaunchConfiguration("use_ptz")
|
||||
|
||||
ld = LaunchDescription()
|
||||
@@ -16,7 +18,23 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument(
|
||||
"connector",
|
||||
default_value="auto",
|
||||
description="Connector parameter for anchor node (default: auto)",
|
||||
description="Connector parameter for anchor node (default: 'auto')",
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
DeclareLaunchArgument(
|
||||
"serial_override",
|
||||
default_value="",
|
||||
description="Serial port override parameter for anchor node (default: '')",
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
DeclareLaunchArgument(
|
||||
"can_override",
|
||||
default_value="auto",
|
||||
description="CAN network override parameter for anchor node (default: '')",
|
||||
)
|
||||
)
|
||||
|
||||
@@ -82,6 +100,8 @@ def generate_launch_description():
|
||||
{
|
||||
"launch_mode": "anchor",
|
||||
"connector": connector,
|
||||
"serial_override": serial_override,
|
||||
"can_override": can_override,
|
||||
}
|
||||
],
|
||||
on_exit=Shutdown(),
|
||||
|
||||
Reference in New Issue
Block a user