From 386bdc555b64ad68e0e618c131312514a03cff0f Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Tue, 27 Aug 2024 12:57:29 -0400 Subject: [PATCH 1/7] Initial can_bridges add --- clearpath_config/platform/can.py | 112 ++++++++++++++++++++++++++ clearpath_config/platform/platform.py | 21 ++++- 2 files changed, 132 insertions(+), 1 deletion(-) create mode 100644 clearpath_config/platform/can.py diff --git a/clearpath_config/platform/can.py b/clearpath_config/platform/can.py new file mode 100644 index 0000000..ea1c221 --- /dev/null +++ b/clearpath_config/platform/can.py @@ -0,0 +1,112 @@ +from clearpath_config.common.types.list import ListConfig +from typing import List + + +class CANBridge: + INTERFACE = "interface" + ENABLE_CAN_FD = "enable_can_fd" + INTERVAL = "interval" + USE_BUS_TIME = "use_bus_time" + FILTERS = "filters" + AUTO_CONFIGURE = "auto_configure" + AUTO_ACTIVATE = "auto_activate" + TOPIC = "topic" + + DEFAULTS = { + INTERFACE: "can0", + ENABLE_CAN_FD: False, + INTERVAL: 0.01, + USE_BUS_TIME: False, + FILTERS: "0:0", + AUTO_CONFIGURE: True, + AUTO_ACTIVATE: True, + TOPIC: "from_can0_bus" + } + + def __init__( + self, + interface: str = DEFAULTS[INTERFACE], + enable_can_fd: bool = DEFAULTS[ENABLE_CAN_FD], + interval: float = DEFAULTS[INTERVAL], + use_bus_time: bool = DEFAULTS[USE_BUS_TIME], + filters: str = DEFAULTS[FILTERS], + auto_configure: bool = DEFAULTS[AUTO_CONFIGURE], + auto_activate: bool = DEFAULTS[AUTO_ACTIVATE], + topic: str = DEFAULTS[TOPIC], + ) -> None: + self.interface = interface + self.enaled_can_fd = enable_can_fd + self.interval = interval + self.use_bus_time = use_bus_time + self.filters = filters + self.auto_configure = auto_configure + self.auto_activate = auto_activate + self.topic = topic + + def to_dict(self) -> dict: + d = dict() + d[self.INTERFACE] = self.interface + d[self.ENABLE_CAN_FD] = self.enaled_can_fd + d[self.INTERVAL] = self.interval + d[self.USE_BUS_TIME] = self.use_bus_time + d[self.FILTERS] = self.filters + d[self.AUTO_CONFIGURE] = self.auto_configure + d[self.AUTO_ACTIVATE] = self.auto_activate + d[self.TOPIC] = self.topic + return d + + def from_dict(self, d: dict) -> None: + if self.INTERFACE in d: + self.interface = d[self.INTERFACE] + if self.ENABLE_CAN_FD in d: + self.enaled_can_fd = d[self.ENABLE_CAN_FD] + if self.INTERVAL in d: + self.interval = d[self.INTERVAL] + if self.USE_BUS_TIME in d: + self.use_bus_time = d[self.USE_BUS_TIME] + if self.FILTERS in d: + self.filters = d[self.FILTERS] + if self.AUTO_CONFIGURE in d: + self.auto_configure = d[self.AUTO_CONFIGURE] + if self.AUTO_ACTIVATE in d: + self.auto_activate = d[self.AUTO_ACTIVATE] + if self.TOPIC in d: + self.topic = d[self.TOPIC] + + +class CANBridgeListConfig(ListConfig[CANBridge, str]): + def __init__(self) -> None: + super().__init__( + uid=lambda obj: obj.interface, + obj_type=CANBridge, + uid_type=str + ) + + +class CANBridgeConfig: + def __init__( + self, + config: dict = {} + ) -> None: + self._can_bridges = CANBridgeListConfig() + self.config = config + + def __add__(self, other): + self._can_bridges.extend(other.get_all()) + return self + + def get_all(self) -> List[CANBridge]: + return self._can_bridges.get_all() + + @property + def config(self): + return [a.to_dict() for a in self.get_all()] + + @config.setter + def config(self, can_bridges: list): + self._can_bridges.remove_all() + for b in can_bridges: + bridge = CANBridge() + bridge.from_dict(b) + self._can_bridges.add(bridge) + diff --git a/clearpath_config/platform/platform.py b/clearpath_config/platform/platform.py index 625452e..584f699 100644 --- a/clearpath_config/platform/platform.py +++ b/clearpath_config/platform/platform.py @@ -33,6 +33,7 @@ from clearpath_config.platform.extras import ExtrasConfig from clearpath_config.platform.attachments.config import AttachmentsConfig from clearpath_config.platform.attachments.mux import AttachmentsConfigMux +from clearpath_config.platform.can import CANBridgeConfig class DescriptionPackagePath(PackagePath): @@ -89,6 +90,7 @@ class PlatformConfig(BaseConfig): CONTROLLER = "controller" ATTACHMENTS = "attachments" + CAN_BRIDGES = "can_bridges" # Extras EXTRAS = "extras" # Generic Robot @@ -104,6 +106,7 @@ class PlatformConfig(BaseConfig): PLATFORM: { CONTROLLER: CONTROLLER, ATTACHMENTS: ATTACHMENTS, + CAN_BRIDGES: CAN_BRIDGES, EXTRAS: EXTRAS, DESCRIPTION: DESCRIPTION, LAUNCH: LAUNCH, @@ -119,6 +122,7 @@ class PlatformConfig(BaseConfig): # PLATFORM CONTROLLER: PS4, ATTACHMENTS: {}, + CAN_BRIDGES: {}, EXTRAS: ExtrasConfig.DEFAULTS, DESCRIPTION: "", LAUNCH: "", @@ -131,7 +135,8 @@ def __init__( self, config: dict = {}, controller: str = DEFAULTS[CONTROLLER], - attachments: str = DEFAULTS[ATTACHMENTS], + attachments: dict = DEFAULTS[ATTACHMENTS], + can_bridges: dict = DEFAULTS[CAN_BRIDGES], battery: dict = DEFAULTS[BATTERY], extras: dict = DEFAULTS[EXTRAS], wheel: dict = DEFAULTS[WHEEL], @@ -140,6 +145,7 @@ def __init__( self._config = {} self.controller = controller self.attachments = attachments + self.can_bridges = can_bridges self._battery = BatteryConfig(battery) self._extras = ExtrasConfig(extras) self.description = self.DEFAULTS[self.DESCRIPTION] @@ -150,6 +156,7 @@ def __init__( setters = { self.KEYS[self.CONTROLLER]: PlatformConfig.controller, self.KEYS[self.ATTACHMENTS]: PlatformConfig.attachments, + self.KEYS[self.CAN_BRIDGES]: PlatformConfig.can_bridges, self.KEYS[self.BATTERY]: PlatformConfig.battery, self.KEYS[self.EXTRAS]: PlatformConfig.extras, self.KEYS[self.WHEEL]: PlatformConfig.wheel, @@ -215,6 +222,18 @@ def attachments(self, value: dict) -> None: self._attachments = AttachmentsConfigMux( self.get_platform_model(), value) + @property + def can_bridges(self) -> CANBridgeConfig: + self.set_config_param( + key=self.KEYS[self.CAN_BRIDGES], + value=self._can_bridges.config + ) + return self._can_bridges + + @can_bridges.setter + def can_bridges(self, value: dict) -> None: + self._can_bridges = CANBridgeConfig(value) + @property def extras(self) -> ExtrasConfig: self.set_config_param( From 6006ac5b415f32f09f0697748b70aed54c771347 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Tue, 27 Aug 2024 13:13:53 -0400 Subject: [PATCH 2/7] rx and tx topics for can bridge --- clearpath_config/platform/can.py | 33 +++++++++++++++++++++++++------- 1 file changed, 26 insertions(+), 7 deletions(-) diff --git a/clearpath_config/platform/can.py b/clearpath_config/platform/can.py index ea1c221..af1b484 100644 --- a/clearpath_config/platform/can.py +++ b/clearpath_config/platform/can.py @@ -10,7 +10,8 @@ class CANBridge: FILTERS = "filters" AUTO_CONFIGURE = "auto_configure" AUTO_ACTIVATE = "auto_activate" - TOPIC = "topic" + TOPIC_RX = "topic_rx" + TOPIC_TX = "topic_tx" DEFAULTS = { INTERFACE: "can0", @@ -20,7 +21,8 @@ class CANBridge: FILTERS: "0:0", AUTO_CONFIGURE: True, AUTO_ACTIVATE: True, - TOPIC: "from_can0_bus" + TOPIC_RX: "can0/rx", + TOPIC_TX: "can0/tx" } def __init__( @@ -32,8 +34,11 @@ def __init__( filters: str = DEFAULTS[FILTERS], auto_configure: bool = DEFAULTS[AUTO_CONFIGURE], auto_activate: bool = DEFAULTS[AUTO_ACTIVATE], - topic: str = DEFAULTS[TOPIC], + topic_rx: str = DEFAULTS[TOPIC_RX], + topic_tx: str = DEFAULTS[TOPIC_TX], ) -> None: + self.topic_rx = topic_rx + self.topic_tx = topic_tx self.interface = interface self.enaled_can_fd = enable_can_fd self.interval = interval @@ -41,7 +46,6 @@ def __init__( self.filters = filters self.auto_configure = auto_configure self.auto_activate = auto_activate - self.topic = topic def to_dict(self) -> dict: d = dict() @@ -52,7 +56,8 @@ def to_dict(self) -> dict: d[self.FILTERS] = self.filters d[self.AUTO_CONFIGURE] = self.auto_configure d[self.AUTO_ACTIVATE] = self.auto_activate - d[self.TOPIC] = self.topic + d[self.TOPIC_RX] = self.topic_rx + d[self.TOPIC_TX] = self.topic_tx return d def from_dict(self, d: dict) -> None: @@ -70,8 +75,22 @@ def from_dict(self, d: dict) -> None: self.auto_configure = d[self.AUTO_CONFIGURE] if self.AUTO_ACTIVATE in d: self.auto_activate = d[self.AUTO_ACTIVATE] - if self.TOPIC in d: - self.topic = d[self.TOPIC] + if self.TOPIC_RX in d: + self.topic_rx = d[self.TOPIC_RX] + if self.TOPIC_TX in d: + self.topic_tx = d[self.TOPIC_TX] + + @property + def interface(self) -> str: + return self._interface + + @interface.setter + def interface(self, interface: str) -> None: + self._interface = interface + if self.topic_rx == self.DEFAULTS[self.TOPIC_RX]: + self.topic_rx = f"{interface}/rx" + if self.topic_tx == self.DEFAULTS[self.TOPIC_TX]: + self.topic_tx = f"{interface}/tx" class CANBridgeListConfig(ListConfig[CANBridge, str]): From bd4777c31d7bbc3c8086bc94f19ede97072fb0e1 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Wed, 9 Oct 2024 10:57:36 -0400 Subject: [PATCH 3/7] Add header --- clearpath_config/platform/can.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/clearpath_config/platform/can.py b/clearpath_config/platform/can.py index af1b484..7a9f732 100644 --- a/clearpath_config/platform/can.py +++ b/clearpath_config/platform/can.py @@ -1,3 +1,30 @@ +# Software License Agreement (BSD) +# +# @author Luis Camero +# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. from clearpath_config.common.types.list import ListConfig from typing import List From 1a55b77dfb5fe5bdf48718c5563253d139df99d5 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Thu, 14 Nov 2024 17:05:27 -0500 Subject: [PATCH 4/7] Add ur_arm --- clearpath_config/manipulators/types/arms.py | 132 ++++++++++++++++++-- 1 file changed, 120 insertions(+), 12 deletions(-) diff --git a/clearpath_config/manipulators/types/arms.py b/clearpath_config/manipulators/types/arms.py index 5135b76..51879b5 100644 --- a/clearpath_config/manipulators/types/arms.py +++ b/clearpath_config/manipulators/types/arms.py @@ -37,15 +37,19 @@ class BaseArm(BaseManipulator): MANIPULATOR_MODEL = "base" MANIPULATOR_TYPE = "arm" - IP_ADDRESS = "192.168.131.40" - IP_PORT = 10000 + IP_ADDRESS = "ip" + IP_PORT = "port" + DEFAULT_IP_ADDRESS = "192.168.131.40" + DEFAULT_IP_PORT = 10000 + + URDF_PARAMETERS = {} def __init__( self, idx: int = None, name: str = None, - ip: str = IP_ADDRESS, - port: int = IP_PORT, + ip: str = DEFAULT_IP_ADDRESS, + port: int = DEFAULT_IP_PORT, ros_parameters: dict = BaseManipulator.ROS_PARAMETERS, ros_parameters_template: dict = BaseManipulator.ROS_PARAMETERS_TEMPLATE, parent: str = Accessory.PARENT, @@ -60,17 +64,19 @@ def __init__( self.ip = IP(ip) # IP Port self.port = Port(port) + # URDF Parameters + self.urdf_parameters = dict(self.URDF_PARAMETERS) @classmethod def get_ip_from_idx(cls, idx: int) -> str: - ip = cls.IP_ADDRESS.split('.') + ip = cls.DEFAULT_IP_ADDRESS.split('.') network_id = ip[0:3] host_id = int(ip[-1]) + idx return '.'.join(network_id) + '.' + str(host_id) def set_idx(self, idx: int) -> None: super().set_idx(idx) - if 'ip' not in self.config: + if self.IP_ADDRESS not in self.config: self.ip = self.get_ip_from_idx(idx) if self.gripper: self.gripper.name = self.name + '_gripper' @@ -94,12 +100,15 @@ def port(self, port: int) -> None: def to_dict(self) -> dict: d = super().to_dict() - d['ip'] = self.ip - d['port'] = self.port + d[self.IP_ADDRESS] = self.ip + d[self.IP_PORT] = self.port if self.gripper: d['gripper'] = self.gripper.to_dict() else: d['gripper'] = None + for k, v in self.urdf_parameters.items(): + if v: + d[k] = v return d def from_dict(self, d: dict) -> None: @@ -111,10 +120,20 @@ def from_dict(self, d: dict) -> None: self.gripper.set_name('%s_gripper' % self.get_name()) if 'parent' not in d['gripper']: self.gripper.set_parent('%s_end_effector_link' % self.get_name()) - if 'ip' in d: - self.ip = d['ip'] - if 'port' in d: - self.port = d['port'] + if self.IP_ADDRESS in d: + self.ip = d[self.IP_ADDRESS] + if self.IP_PORT in d: + self.port = d[self.IP_PORT] + for k in self.urdf_parameters: + if k in d: + self.urdf_parameters[k] = d[k] + + def get_urdf_parameters(self) -> dict: + d = {} + for k, v in self.urdf_parameters.items(): + if v: + d[k] = v + return d class KinovaGen3Dof6(BaseArm): @@ -129,15 +148,104 @@ class KinovaGen3Lite(BaseArm): MANIPULATOR_MODEL = "kinova_gen3_lite" +class UniversalRobots(BaseArm): + MANIPULATOR_MODEL = "universal_robots" + + # Description Variables + UR_TYPE = 'ur_type' + INITIAL_POSITIONS = 'initial_positions' + INITIAL_POSITIONS_FILE = 'initial_positions_file' + JOINT_LIMITS_PARAMETERS_FILE = 'joint_limits_parameters_file' + KINEMATICS_PARAMETERS_FILE = 'kinematics_parameters_file' + PHYSICAL_PARAMETERS_FILE = 'physical_parameters_file' + VISUAL_PARAMETERS_FILE = 'visual_parameters_file' + SAFETY_LIMITS = 'safety_limits' + SAFETY_POS_MARGIN = 'safety_pos_margin' + SAFETY_K_POSITION = 'safety_k_position' + # Control Parameters + GENERATE_ROS2_CONTROL_TAG = 'generate_ros2_control_tag' + HEADLESS_MODE = 'headless_mode' + IP_ADDRESS = 'robot_ip' + SCRIPT_FILENAME = 'script_filename' + OUTPUT_RECIPE_FILENAME = 'output_recipe_filename' + INPUT_RECIPE_FILENAME = 'input_recipe_filename' + REVERSE_IP = 'reverse_ip' + SCRIPT_COMMAND_PORT = 'script_command_port' + REVERSE_PORT = 'reverse_port' + SCRIPT_SENDER_PORT = 'script_sender_port' + TRAJECTORY_PORT = 'trajectory_port' + TRANSMISSION_HW_INTERFACE = 'transmission_hw_interface' + NON_BLOCKING_READ = 'non_blocking_read' + KEEP_ALIVE_COUNT = 'keep_alive_count' + # Tool Communication Parameters + USE_TOOL_COMMUNICATION = 'use_tool_communication' + TOOL_VOLTAGE = 'tool_voltage' + TOOL_PARITY = 'tool_parity' + TOOL_BAUD_RATE = 'tool_baud_rate' + TOOL_STOP_BITS = 'tool_stop_bits' + TOOL_RX_IDLE_CHARS = 'tool_rx_idle_chars' + TOOL_TX_IDLE_CHARS = 'tool_tx_idle_chars' + TOOL_DEVICE_NAME = 'tool_device_name' + TOOL_TCP_PORT = 'tool_tcp_port' + # Simulation Parameters + USE_FAKE_HARDWARE = 'use_fake_hardware' + FAKE_SENSOR_COMMANDS = 'fake_sensor_commands' + SIM_GAZEBO = 'sim_gazebo' + SIM_IGNITION = 'sim_ignition' + + # URDF Parameters + URDF_PARAMETERS = { + UR_TYPE: '', + INITIAL_POSITIONS: '', + INITIAL_POSITIONS_FILE: '', + JOINT_LIMITS_PARAMETERS_FILE: '', + KINEMATICS_PARAMETERS_FILE: '', + PHYSICAL_PARAMETERS_FILE: '', + VISUAL_PARAMETERS_FILE: '', + SAFETY_LIMITS: '', + SAFETY_POS_MARGIN: '', + SAFETY_K_POSITION: '', + GENERATE_ROS2_CONTROL_TAG: '', + HEADLESS_MODE: '', + IP_ADDRESS: '', + SCRIPT_FILENAME: '', + OUTPUT_RECIPE_FILENAME: '', + INPUT_RECIPE_FILENAME: '', + REVERSE_IP: '', + SCRIPT_COMMAND_PORT: '', + REVERSE_PORT: '', + SCRIPT_SENDER_PORT: '', + TRAJECTORY_PORT: '', + TRANSMISSION_HW_INTERFACE: '', + NON_BLOCKING_READ: '', + KEEP_ALIVE_COUNT: '', + USE_TOOL_COMMUNICATION: '', + TOOL_VOLTAGE: '', + TOOL_PARITY: '', + TOOL_BAUD_RATE: '', + TOOL_STOP_BITS: '', + TOOL_RX_IDLE_CHARS: '', + TOOL_TX_IDLE_CHARS: '', + TOOL_DEVICE_NAME: '', + TOOL_TCP_PORT: '', + USE_FAKE_HARDWARE: '', + FAKE_SENSOR_COMMANDS: '', + SIM_GAZEBO: '', + SIM_IGNITION: '', + } + + class Arm(): KINOVA_GEN3_6DOF = KinovaGen3Dof6.MANIPULATOR_MODEL KINOVA_GEN3_7DOF = KinovaGen3Dof7.MANIPULATOR_MODEL KINOVA_GEN3_LITE = KinovaGen3Lite.MANIPULATOR_MODEL + UNIVERSAL_ROBOTS = UniversalRobots.MANIPULATOR_MODEL MODEL = { KINOVA_GEN3_6DOF: KinovaGen3Dof6, KINOVA_GEN3_7DOF: KinovaGen3Dof7, KINOVA_GEN3_LITE: KinovaGen3Lite, + UNIVERSAL_ROBOTS: UniversalRobots, } @classmethod From e64cd4d61f545c8eae0963189a664c89e4e2729a Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 18 Nov 2024 10:08:05 -0500 Subject: [PATCH 5/7] Remove empty line at EoF --- clearpath_config/platform/can.py | 1 - 1 file changed, 1 deletion(-) diff --git a/clearpath_config/platform/can.py b/clearpath_config/platform/can.py index 7a9f732..a55f8fb 100644 --- a/clearpath_config/platform/can.py +++ b/clearpath_config/platform/can.py @@ -155,4 +155,3 @@ def config(self, can_bridges: list): bridge = CANBridge() bridge.from_dict(b) self._can_bridges.add(bridge) - From 8e1848a971e1b8ec9477e86aa029700554573e61 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton <59611394+civerachb-cpr@users.noreply.github.com> Date: Tue, 19 Nov 2024 17:10:59 -0500 Subject: [PATCH 6/7] Add support for Axis cameras (#90) * Add the initial AxisCamera class with all ROS parameters defined in axis_camera's launch files & nodes * Remove duplicate argument * Add the AxisCamera class to the sensors generator * Add the serial to the axis camera's template * Add serial to the template keys too * Add serial getter/setter. Use empty string as default serial * Refactoring, set the property to the value for the template * frame_width -> width, frame_height -> height * Rename setter * Make the scales & offsets floats by default * Add the TF prefix parameter * Add the camera_info_url parameter * camera_num -> camera * Note that the serial isn't used, fix the name of the PTZ teleop parameter * Add the remaining camera topics to the Topics object * image_raw -> image * Add axis_camera sample * Linting fixes * End docstring with `.` --- .../sample/sensors/axis_camera.yaml | 50 ++ clearpath_config/sensors/sensors.py | 3 + clearpath_config/sensors/types/cameras.py | 623 ++++++++++++++++++ 3 files changed, 676 insertions(+) create mode 100644 clearpath_config/sample/sensors/axis_camera.yaml diff --git a/clearpath_config/sample/sensors/axis_camera.yaml b/clearpath_config/sample/sensors/axis_camera.yaml new file mode 100644 index 0000000..32f9a5f --- /dev/null +++ b/clearpath_config/sample/sensors/axis_camera.yaml @@ -0,0 +1,50 @@ +serial_number: a200-0000 +version: 0 +sensors: + camera: + - model: axis_camera + urdf_enabled: true + launch_enabled: true + parent: base_link + xyz: [0.0, 0.0, 0.0] + rpy: [0.0, 0.0, 0.0] + ros_parameters: + axis_camera: + hostname: "192.168.131.10" + http_port: 80 + username: "root" + password: "" + width: 640 + height: 480 + fps: 20 + tf_prefix: "axis" + camera_info_url: "" + use_encrypted_password : False + camera : 1 + ir: False + defog: False + wiper: False + ptz: True + min_pan: -3.141592653589793 + max_pan: 3.141592653589793 + min_tilt: 0.0 + max_tilt: 1.5707963267948966 + min_zoom: 1 + max_zoom: 24 + max_pan_speed: 2.61 + max_tilt_speed: 2.61 + ptz_teleop: True + button_enable_pan_tilt : -1 + button_enable_zoom : -1 + axis_pan : 3 + axis_tilt : 4 + invert_tilt : False + axis_zoom_in: 5 + axis_zoom_out: 2 + zoom_in_offset: -1.0 + zoom_out_offset: -1.0 + zoom_in_scale: -0.5 + zoom_out_scale: 0.5 + scale_pan : 2.61 + scale_tilt : 2.61 + scale_zoom : 100.0 \ No newline at end of file diff --git a/clearpath_config/sensors/sensors.py b/clearpath_config/sensors/sensors.py index 2726ab6..f6cefe3 100644 --- a/clearpath_config/sensors/sensors.py +++ b/clearpath_config/sensors/sensors.py @@ -32,6 +32,7 @@ from clearpath_config.common.utils.dictionary import flip_dict from clearpath_config.sensors.types.sensor import BaseSensor from clearpath_config.sensors.types.cameras import ( + AxisCamera, BaseCamera, FlirBlackfly, IntelRealsense, @@ -93,12 +94,14 @@ def __new__(cls, model: str) -> BaseIMU: class Camera(): + AXIS_CANERA = AxisCamera.SENSOR_MODEL FLIR_BLACKFLY = FlirBlackfly.SENSOR_MODEL INTEL_REALSENSE = IntelRealsense.SENSOR_MODEL STEREOLABS_ZED = StereolabsZed.SENSOR_MODEL LUXONIS_OAKD = LuxonisOAKD.SENSOR_MODEL MODEL = { + AXIS_CANERA: AxisCamera, FLIR_BLACKFLY: FlirBlackfly, INTEL_REALSENSE: IntelRealsense, STEREOLABS_ZED: StereolabsZed, diff --git a/clearpath_config/sensors/types/cameras.py b/clearpath_config/sensors/types/cameras.py index 007d10a..d433e6e 100644 --- a/clearpath_config/sensors/types/cameras.py +++ b/clearpath_config/sensors/types/cameras.py @@ -25,6 +25,8 @@ # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from math import pi + from clearpath_config.common.types.accessory import Accessory from clearpath_config.sensors.types.sensor import BaseSensor from clearpath_config.common.utils.dictionary import extend_flat_dict @@ -1053,3 +1055,624 @@ def get_fps(self) -> float: def set_fps(self, fps: float) -> None: self.fps = fps + + +class AxisCamera(BaseCamera): + """PTZ and fixed cameras that use the axis_camera driver.""" + + SERIAL = '' + + SENSOR_MODEL = 'axis_camera' + TF_PREFIX = 'axis' + + HOSTNAME = '192.168.10.0' + CAMERA_INFO_URL = '' + + DOME_FIXED = 'dome_fixed' + DOME_PTZ = 'dome_ptz' + Q62 = 'q62' + + DEVICE_TYPE = DOME_PTZ + DEVICE_TYPES = [ + Q62, + DOME_PTZ, + DOME_FIXED, + ] + + HOSTNAME = '192.168.10.0' + HTTP_PORT = 80 + USERNAME = 'root' + PASSWORD = '' + USE_ENCRYPTED_PASSWORD = True + + CAMERA = 1 + WIDTH = 640 + HEIGHT = 480 + FPS = 20 + + ENABLE_PTZ = True + MIN_PAN = -pi + MAX_PAN = pi + MIN_TILT = -pi/2 + MAX_TILT = pi/2 + MIN_ZOOM = 1 + MAX_ZOOM = 24 + MAX_PAN_SPEED = 2.61 + MAX_TILT_SPEED = 2.61 + + ENABLE_IR = False + ENABLE_WPER = False + ENABLE_DEFOG = False + + ENABLE_PTZ_TELEOP = False + BUTTON_ENABLE_PAN_TILT = -1 + BUTTON_ENABLE_ZOOM = -1 + AXIS_PAN = -1 + AXIS_TILT = -1 + INVERT_TILT = False + AXIS_ZOOM_IN = -1 + AXIS_ZOOM_OUT = -1 + ZOOM_IN_OFFSET = 0.0 + ZOOM_OUT_OFFSET = 0.0 + ZOOM_IN_SCALE = 1.0 + ZOOM_OUT_SCALE = 1.0 + SCALE_PAN = 1.0 + SCALE_TILT = 1.0 + SCALE_ZOOM = 1.0 + + class ROS_PARAMETER_KEYS: + SERIAL = 'axis_camera.serial' # required by superclass, not used locally + + TF_PREFIX = 'axis_camera.tf_prefix' + + HOSTNAME = 'axis_camera.hostname' + HTTP_PORT = 'axis_camera.http_port' + CAMERA_INFO_URL = 'axis_camera.camera_info_url' + + USERNAME = 'axis_camera.username' + PASSWORD = 'axis_camera.password' + USE_ENCRYPTED_PASSWORD = 'axis_camera.use_encrypted_password' + + CAMERA = 'axis_camera.camera' + WIDTH = 'axis_camera.width' + HEIGHT = 'axis_camera.height' + FPS = 'axis_camera.fps' + + # PTZ + ENABLE_PTZ = 'axis_camera.ptz' + MIN_PAN = 'axis_camera.min_pan' + MAX_PAN = 'axis_camera.max_pan' + MIN_TILT = 'axis_camera.min_tilt' + MAX_TILT = 'axis_camera.max_tilt' + MIN_ZOOM = 'axis_camera.min_zoom' + MAX_ZOOM = 'axis_camera.max_zoom' + MAX_PAN_SPEED = 'axis_camera.max_pan_speed' + MAX_TILT_SPEED = 'axis_camera.max_tilt_speed' + + # Q62 additional services + ENABLE_IR = 'axis_camera.ir' + ENABLE_WIPER = 'axis_camera.wiper' + ENABLE_DEFOG = 'axis_camera.defog' + + # PTZ Teleop (see axis_camera/config/teleop_ps4.yaml) + ENABLE_PTZ_TELEOP = 'axis_camera.ptz_teleop' + BUTTON_ENABLE_PAN_TILT = 'axis_camera.button_enable_pan_tilt' + BUTTON_ENABLE_ZOOM = 'axis_camera.button_enable_zoom' + AXIS_PAN = 'axis_camera.axis_pan' + AXIS_TILT = 'axis_camera.axis_tilt' + INVERT_TILT = 'axis_camera.invert_tilt' + AXIS_ZOOM_IN = 'axis_camera.axis_zoom_in' + AXIS_ZOOM_OUT = 'axis_camera.axis_zoom_out' + ZOOM_IN_OFFSET = 'axis_camera.zoom_in_offset' + ZOOM_OUT_OFFSET = 'axis_camera.zoom_out_offset' + ZOOM_IN_SCALE = 'axis_camera.zoom_in_scale' + ZOOM_OUT_SCALE = 'axis_camera.zoom_out_scale' + SCALE_PAN = 'axis_camera.scale_pan' + SCALE_TILT = 'axis_camera.scale_tilt' + SCALE_ZOOM = 'axis_camera.scale_zoom' + + class TOPICS: + IMAGE = 'image/compressed' + CAMERA_INFO = 'camera_info' + + AUTOFOCUS = 'autofocus' + AUTOIRIS = 'autoiris' + BRIGHTNESS = 'brightness' + FOCUS = 'focus' + IRIS = 'iris' + JOINT_STATES = 'joint_states' + PTZ_STATE = 'ptz_state' + + def __init__( + self, + idx: int = None, + name: str = None, + topic: str = BaseCamera.TOPIC, + fps: int = FPS, + serial: str = SERIAL, + device_type: str = Q62, + + hostname: str = HOSTNAME, + http_port: int = HTTP_PORT, + username: str = USERNAME, + password: str = PASSWORD, + use_encrypted_password: bool = USE_ENCRYPTED_PASSWORD, + camera_info_url: str = CAMERA_INFO_URL, + + camera: int = CAMERA, + width: int = WIDTH, + height: int = HEIGHT, + + enable_ptz: bool = ENABLE_PTZ, + min_pan: float = MIN_PAN, + max_pan: float = MAX_PAN, + min_tilt: float = MIN_TILT, + max_tilt: float = MAX_TILT, + min_zoom: float = MIN_ZOOM, + max_zoom: float = MAX_ZOOM, + max_pan_speed: float = MAX_PAN_SPEED, + max_tilt_speed: float = MAX_TILT_SPEED, + + enable_ir: bool = ENABLE_IR, + enable_wiper: bool = ENABLE_WPER, + enable_defog: bool = ENABLE_DEFOG, + + enable_ptz_teleop: bool = ENABLE_PTZ_TELEOP, + button_enable_pan_tilt: int = BUTTON_ENABLE_PAN_TILT, + button_enable_zoom: int = BUTTON_ENABLE_ZOOM, + axis_pan: int = AXIS_PAN, + axis_tilt: int = AXIS_TILT, + invert_tilt: bool = INVERT_TILT, + axis_zoom_in: int = AXIS_ZOOM_IN, + axis_zoom_out: int = AXIS_ZOOM_OUT, + zoom_in_offset: float = ZOOM_IN_OFFSET, + zoom_out_offset: float = ZOOM_OUT_OFFSET, + zoom_in_scale: float = ZOOM_IN_SCALE, + zoom_out_scale: float = ZOOM_OUT_SCALE, + scale_pan: float = SCALE_PAN, + scale_tilt: float = SCALE_TILT, + scale_zoom: float = SCALE_ZOOM, + + urdf_enabled: bool = BaseSensor.URDF_ENABLED, + launch_enabled: bool = BaseSensor.LAUNCH_ENABLED, + ros_parameters: dict = BaseSensor.ROS_PARAMETERS, + ros_parameters_template: dict = BaseSensor.ROS_PARAMETERS_TEMPLATE, + parent: str = Accessory.PARENT, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY + ) -> None: + # ROS Parameter Template + ros_parameters_template = { + self.ROS_PARAMETER_KEYS.SERIAL: AxisCamera.serial, + + self.ROS_PARAMETER_KEYS.TF_PREFIX: AxisCamera.tf_prefix, + + self.ROS_PARAMETER_KEYS.HOSTNAME: AxisCamera.hostname, + self.ROS_PARAMETER_KEYS.HTTP_PORT: AxisCamera.http_port, + self.ROS_PARAMETER_KEYS.CAMERA_INFO_URL: AxisCamera.camera_info_url, + + self.ROS_PARAMETER_KEYS.USERNAME: AxisCamera.username, + self.ROS_PARAMETER_KEYS.PASSWORD: AxisCamera.password, + self.ROS_PARAMETER_KEYS.USE_ENCRYPTED_PASSWORD: AxisCamera.use_encrypted_password, + + self.ROS_PARAMETER_KEYS.CAMERA: AxisCamera.camera, + self.ROS_PARAMETER_KEYS.WIDTH: AxisCamera.width, + self.ROS_PARAMETER_KEYS.HEIGHT: AxisCamera.height, + self.ROS_PARAMETER_KEYS.FPS: AxisCamera.fps, + + # PTZ + self.ROS_PARAMETER_KEYS.ENABLE_PTZ: AxisCamera.enable_ptz, + self.ROS_PARAMETER_KEYS.MIN_PAN: AxisCamera.min_pan, + self.ROS_PARAMETER_KEYS.MAX_PAN: AxisCamera.max_pan, + self.ROS_PARAMETER_KEYS.MIN_TILT: AxisCamera.min_tilt, + self.ROS_PARAMETER_KEYS.MAX_TILT: AxisCamera.max_tilt, + self.ROS_PARAMETER_KEYS.MIN_ZOOM: AxisCamera.min_zoom, + self.ROS_PARAMETER_KEYS.MAX_ZOOM: AxisCamera.max_zoom, + self.ROS_PARAMETER_KEYS.MAX_PAN_SPEED: AxisCamera.max_pan_speed, + self.ROS_PARAMETER_KEYS.MAX_TILT_SPEED: AxisCamera.max_tilt_speed, + + # Q62 additional services + self.ROS_PARAMETER_KEYS.ENABLE_IR: AxisCamera.enable_ir, + self.ROS_PARAMETER_KEYS.ENABLE_WIPER: AxisCamera.enable_wiper, + self.ROS_PARAMETER_KEYS.ENABLE_DEFOG: AxisCamera.enable_defog, + + # PTZ Teleop (see axis_camera/config/teleop_ps4.yaml) + self.ROS_PARAMETER_KEYS.ENABLE_PTZ_TELEOP: AxisCamera.enable_ptz_teleop, + self.ROS_PARAMETER_KEYS.BUTTON_ENABLE_PAN_TILT: AxisCamera.button_enable_pan_tilt, + self.ROS_PARAMETER_KEYS.BUTTON_ENABLE_ZOOM: AxisCamera.button_enable_zoom, + self.ROS_PARAMETER_KEYS.AXIS_PAN: AxisCamera.axis_pan, + self.ROS_PARAMETER_KEYS.AXIS_TILT: AxisCamera.axis_tilt, + self.ROS_PARAMETER_KEYS.INVERT_TILT: AxisCamera.invert_tilt, + self.ROS_PARAMETER_KEYS.AXIS_ZOOM_IN: AxisCamera.axis_zoom_in, + self.ROS_PARAMETER_KEYS.AXIS_ZOOM_OUT: AxisCamera.axis_zoom_out, + self.ROS_PARAMETER_KEYS.ZOOM_IN_OFFSET: AxisCamera.zoom_in_offset, + self.ROS_PARAMETER_KEYS.ZOOM_OUT_OFFSET: AxisCamera.zoom_out_offset, + self.ROS_PARAMETER_KEYS.ZOOM_IN_SCALE: AxisCamera.zoom_in_scale, + self.ROS_PARAMETER_KEYS.ZOOM_OUT_SCALE: AxisCamera.zoom_out_scale, + self.ROS_PARAMETER_KEYS.SCALE_PAN: AxisCamera.scale_pan, + self.ROS_PARAMETER_KEYS.SCALE_TILT: AxisCamera.scale_tilt, + self.ROS_PARAMETER_KEYS.SCALE_ZOOM: AxisCamera.scale_zoom, + } + # Initialization + self.device_type: str = device_type + super().__init__( + idx, + name, + topic, + fps, + serial, + urdf_enabled, + launch_enabled, + ros_parameters, + ros_parameters_template, + parent, + xyz, + rpy + ) + self.tf_prefix = name + + self.hostname = hostname + self.http_port = http_port + self.username = username + self.password = password + self.use_encrypted_password = use_encrypted_password + self.camera_info_url = camera_info_url + + self.camera = camera + self.width = width + self.height = height + + self.enable_ptz = enable_ptz + self.min_pan = min_pan + self.max_pan = max_pan + self.min_tilt = min_tilt + self.max_tilt = max_tilt + self.min_zoom = min_zoom + self.max_zoom = max_zoom + self.max_pan_speed = max_pan_speed + self.max_tilt_speed = max_tilt_speed + + self.enable_ir = enable_ir + self.enable_wiper = enable_wiper + self.enable_defog = enable_defog + + self.enable_ptz_teleop = enable_ptz_teleop + self.button_enable_pan_tilt = button_enable_pan_tilt + self.button_enable_zoom = button_enable_zoom + self.axis_pan = axis_pan + self.axis_tilt = axis_tilt + self.invert_tilt = invert_tilt + self.axis_zoom_in = axis_zoom_in + self.axis_zoom_out = axis_zoom_out + self.zoom_in_offset = zoom_in_offset + self.zoom_out_offset = zoom_out_offset + self.zoom_in_scale = zoom_in_scale + self.zoom_out_scale = zoom_out_scale + self.scale_pan = scale_pan + self.scale_tilt = scale_tilt + self.scale_zoom = scale_zoom + + @property + def device_type(self) -> str: + return self._device_type + + @device_type.setter + def device_type(self, device_type: str) -> None: + assert device_type in self.DEVICE_TYPES, ( + "Device type '%s' is not one of '%s'" % ( + device_type, + self.DEVICE_TYPES + ) + ) + self._device_type = device_type + + @property + def tf_prefix(self) -> str: + return self._tf_prefix + + @tf_prefix.setter + def tf_prefix(self, prefix: str) -> None: + self._tf_prefix = prefix + + @property + def hostname(self) -> str: + return self._hostname + + @hostname.setter + def hostname(self, hostname: str) -> None: + self._hostname = hostname + + @property + def http_port(self) -> int: + return self._http_port + + @http_port.setter + def http_port(self, port: int) -> None: + assert port >= 0 and port <= 65535, ( + f'Port {port} must be in range [0, 65535]' + ) + self._http_port = port + + @property + def camera_info_url(self) -> str: + return self._camera_info_url + + @camera_info_url.setter + def camera_info_url(self, url: str) -> None: + self._camera_info_url = url + + @property + def username(self) -> str: + return self._username + + @username.setter + def username(self, username: str) -> None: + assert len(username) > 0, 'Username cannot be empty' + self._username = username + + @property + def password(self) -> str: + return self._password + + @password.setter + def password(self, password: str) -> None: + self._password = password + + @property + def use_encrypted_password(self) -> bool: + return self._use_encrypted_password + + @use_encrypted_password.setter + def use_encrypted_password(self, encrypt: bool) -> None: + self._use_encrypted_password = encrypt + + @property + def camera(self) -> int: + return self._camera + + @camera.setter + def camera(self, num: int) -> None: + assert num >= 0, f'Camera number {num} cannot be negative' + self._camera = num + + @property + def width(self) -> int: + return self._width + + @width.setter + def width(self, width: int) -> None: + assert width > 0, f'Frame width {width} must be positive' + self._width = width + + @property + def height(self) -> int: + return self._height + + @height.setter + def height(self, height: int) -> None: + assert height > 0, f'Frame height {height} must be positive' + self._height = height + + @property + def enable_ptz(self) -> bool: + return self._enable_ptz + + @enable_ptz.setter + def enable_ptz(self, enable: bool) -> None: + self._enable_ptz = enable + + @property + def min_pan(self) -> float: + return self._min_pan + + @min_pan.setter + def min_pan(self, pan: float) -> None: + assert pan >= -pi and pan <= pi, f'Min pan {pan} must be in range [-pi, pi]' + self._min_pan = pan + + @property + def max_pan(self) -> float: + return self._max_pan + + @max_pan.setter + def max_pan(self, pan: float) -> None: + assert pan >= -pi and pan <= pi, f'Max pan {pan} must be in range [-pi, pi]' + self._max_pan = pan + + @property + def min_tilt(self) -> float: + return self._min_tilt + + @min_tilt.setter + def min_tilt(self, tilt: float) -> None: + assert tilt >= -pi/2 and tilt <= pi/2, f'Min tilt {tilt} must be in range [-pi/2, pi/2]' + self._min_tilt = tilt + + @property + def max_tilt(self) -> float: + return self._max_tilt + + @max_tilt.setter + def max_tilt(self, tilt: float) -> None: + assert tilt >= -pi/2 and tilt <= pi/2, f'Max tilt {tilt} must be in range [-pi/2, pi/2]' + self._max_tilt = tilt + + @property + def min_zoom(self) -> float: + return self._min_zoom + + @min_zoom.setter + def min_zoom(self, zoom: float) -> None: + assert zoom > 0, f'Min zoom {zoom} must be positive' + self._min_zoom = zoom + + @property + def max_zoom(self) -> float: + return self._max_zoom + + @max_zoom.setter + def max_zoom(self, zoom: float) -> None: + assert zoom > 0, f'Max zoom {zoom} must be positive' + self._max_zoom = zoom + + @property + def max_pan_speed(self) -> float: + return self._max_pan_speed + + @max_pan_speed.setter + def max_pan_speed(self, speed: float) -> None: + assert speed > 0, f'Max pan speed {speed} must be positive' + self._max_pan_speed = speed + + @property + def max_tilt_speed(self) -> float: + return self._max_tilt_speed + + @max_tilt_speed.setter + def max_tilt_speed(self, speed: float) -> None: + assert speed > 0, f'Max tilt speed {speed} must be positive' + self._max_tilt_speed = speed + + @property + def enable_ir(self) -> bool: + return self._enable_ir + + @enable_ir.setter + def enable_ir(self, enable: bool) -> None: + self._enable_ir = enable + + @property + def enable_wiper(self) -> bool: + return self._enable_wiper + + @enable_wiper.setter + def enable_wiper(self, enable: bool) -> None: + self._enable_wiper = enable + + @property + def enable_defog(self) -> bool: + return self._enable_defog + + @enable_defog.setter + def enable_defog(self, enable: bool) -> None: + self._enable_defog = enable + + @property + def enable_ptz_teleop(self) -> bool: + return self._enable_ptz_teleop + + @enable_ptz_teleop.setter + def enable_ptz_teleop(self, enable: bool) -> None: + self._enable_ptz_teleop = enable + + @property + def invert_tilt(self) -> bool: + return self._invert_tilt + + @invert_tilt.setter + def invert_tilt(self, enable: bool) -> None: + self._invert_tilt = enable + + @property + def button_enable_pan_tilt(self) -> int: + return self._button_enable_pan_tilt + + @button_enable_pan_tilt.setter + def button_enable_pan_tilt(self, button: int) -> None: + self._button_enable_pan_tilt = button + + @property + def button_enable_zoom(self) -> int: + return self._button_enable_zoom + + @button_enable_zoom.setter + def button_enable_zoom(self, button: int) -> None: + self._button_enable_zoom = button + + @property + def axis_pan(self) -> int: + return self._axis_pan + + @axis_pan.setter + def axis_pan(self, axis: int) -> None: + self._axis_pan = axis + + @property + def axis_tilt(self) -> int: + return self._axis_tilt + + @axis_tilt.setter + def axis_tilt(self, axis: int) -> None: + self._axis_tilt = axis + + @property + def axis_zoom_in(self) -> int: + return self._axis_zoom_in + + @axis_zoom_in.setter + def axis_zoom_in(self, axis: int) -> None: + self._axis_zoom_in = axis + + @property + def axis_zoom_out(self) -> int: + return self._axis_zoom_out + + @axis_zoom_out.setter + def axis_zoom_out(self, axis: int) -> None: + self._axis_zoom_out = axis + + @property + def zoom_in_offset(self) -> float: + return self._zoom_in_offset + + @zoom_in_offset.setter + def zoom_in_offset(self, offset: float) -> None: + self._zoom_in_offset = offset + + @property + def zoom_out_offset(self) -> float: + return self._zoom_out_offset + + @zoom_out_offset.setter + def zoom_out_offset(self, offset: float) -> None: + self._zoom_out_offset = offset + + @property + def zoom_in_scale(self) -> float: + return self._zoom_in_scale + + @zoom_in_scale.setter + def zoom_in_scale(self, scale: float) -> None: + self._zoom_in_scale = scale + + @property + def zoom_out_scale(self) -> float: + return self._zoom_out_scale + + @zoom_out_scale.setter + def zoom_out_scale(self, scale: float) -> None: + self._zoom_out_scale = scale + + @property + def scale_pan(self) -> float: + return self._scale_pan + + @scale_pan.setter + def scale_pan(self, scale: float) -> None: + self._scale_pan = scale + + @property + def scale_tilt(self) -> float: + return self._scale_tilt + + @scale_tilt.setter + def scale_tilt(self, scale: float) -> None: + self._scale_tilt = scale + + @property + def scale_zoom(self) -> float: + return self._scale_zoom + + @scale_zoom.setter + def scale_zoom(self, scale: float) -> None: + self._scale_zoom = scale From 61e9a88a9d35ad02eb66cd32591492100cbdb83a Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Fri, 22 Nov 2024 11:47:19 -0500 Subject: [PATCH 7/7] Add default vcan ROS interfaces based on platform --- clearpath_config/platform/can.py | 30 +++++++++++++++++++++++++++ clearpath_config/platform/platform.py | 1 + 2 files changed, 31 insertions(+) diff --git a/clearpath_config/platform/can.py b/clearpath_config/platform/can.py index a55f8fb..e4001bb 100644 --- a/clearpath_config/platform/can.py +++ b/clearpath_config/platform/can.py @@ -25,7 +25,9 @@ # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from clearpath_config.common.types.config import BaseConfig from clearpath_config.common.types.list import ListConfig +from clearpath_config.common.types.platform import Platform from typing import List @@ -130,6 +132,30 @@ def __init__(self) -> None: class CANBridgeConfig: + SINGLE_VCAN_DEFAULT = [ + { + CANBridge.INTERFACE: "vcan0", + CANBridge.ENABLE_CAN_FD: False, + CANBridge.INTERVAL: 0.01, + CANBridge.USE_BUS_TIME: False, + CANBridge.FILTERS: "0:0", + CANBridge.AUTO_CONFIGURE: True, + CANBridge.AUTO_ACTIVATE: True, + } + ] + + DEFAULTS = { + Platform.A200: [], + Platform.DD100: SINGLE_VCAN_DEFAULT, + Platform.DD150: SINGLE_VCAN_DEFAULT, + Platform.DO100: SINGLE_VCAN_DEFAULT, + Platform.DO150: SINGLE_VCAN_DEFAULT, + Platform.GENERIC: [], + Platform.J100: [], + Platform.R100: SINGLE_VCAN_DEFAULT, + Platform.W200: [], + } + def __init__( self, config: dict = {} @@ -155,3 +181,7 @@ def config(self, can_bridges: list): bridge = CANBridge() bridge.from_dict(b) self._can_bridges.add(bridge) + + def update(self, serial_number: bool = False) -> None: + if serial_number: + self.config = self.DEFAULTS[BaseConfig.get_platform_model()] diff --git a/clearpath_config/platform/platform.py b/clearpath_config/platform/platform.py index 584f699..cb72c13 100644 --- a/clearpath_config/platform/platform.py +++ b/clearpath_config/platform/platform.py @@ -192,6 +192,7 @@ def update(self, serial_number=False) -> None: self.template = template # Reload battery self.battery.update(serial_number=serial_number) + self.can_bridges.update(serial_number=serial_number) @property def controller(self) -> str: