diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..5e941e0 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +version: 2 +updates: + - package-ecosystem: github-actions + directory: / + schedule: + interval: weekly + open-pull-requests-limit: 5 + + - package-ecosystem: pip + directory: / + schedule: + interval: weekly + open-pull-requests-limit: 5 diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000..0747586 --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,44 @@ +name: autofix.ci + +on: + pull_request: + push: + branches: + - main + workflow_dispatch: + +permissions: + contents: write + pull-requests: write + +jobs: + ruff: + name: Ruff format + runs-on: ubuntu-22.04 + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref || github.ref_name }} + + - name: Set up Python 3.10 + uses: actions/setup-python@v5 + with: + python-version: '3.10' + cache: pip + cache-dependency-path: requirements-dev.txt + + - name: Install formatter + run: python -m pip install -r requirements-dev.txt + + - name: Format Python + if: github.event_name != 'push' + run: ./scripts/format + + - name: Check Python formatting + if: github.event_name == 'push' + run: ./scripts/check-format + + - name: Commit formatting fixes + if: github.event_name != 'push' + uses: autofix-ci/action@v1 diff --git a/README.md b/README.md index 0ab07e1..bd389f9 100644 --- a/README.md +++ b/README.md @@ -28,3 +28,23 @@ list_property: - list_value2 empyt_list_property: [] ``` + +## Formatting +This package uses Ruff for Python formatting and import ordering. The formatter is configured +for ROS 2's Python style preferences and Ubuntu 22.04 / Python 3.10 compatibility in +`pyproject.toml`. + +Install the formatter: +```bash +python3 -m pip install -r requirements-dev.txt +``` + +Format the repository: +```bash +./scripts/format +``` + +Check formatting without modifying files: +```bash +./scripts/check-format +``` diff --git a/launch/launch_all_vehicles.launch.py b/launch/launch_all_vehicles.launch.py index dc3bc83..78bae82 100644 --- a/launch/launch_all_vehicles.launch.py +++ b/launch/launch_all_vehicles.launch.py @@ -1,29 +1,32 @@ # python imports from pathlib import Path + import yaml # ROS imports from ament_index_python import get_package_share_directory -from launch import LaunchDescription, logging from launch_ros.actions import Node -package_name = "mavinsight" -namespace = "viz" -LOGGER = logging.get_logger("vehicle_launch_logger") -initial_paths_overrides = ["chimera_d_4.yaml", "c2_c130_crash.yaml"] +from launch import LaunchDescription, logging + +package_name = 'mavinsight' +namespace = 'viz' +LOGGER = logging.get_logger('vehicle_launch_logger') +initial_paths_overrides = ['chimera_d_4.yaml', 'c2_c130_crash.yaml'] + def generate_launch_description(): ld = LaunchDescription() share_dir = Path(get_package_share_directory(package_name)) - shared_resources = share_dir / "package_resources" + shared_resources = share_dir / 'package_resources' global_config = shared_resources / 'global_node_config.yaml' # TODO change behavior for empty initial paths override initial_paths = [(shared_resources) / p for p in initial_paths_overrides] - LOGGER.info(f"Initial paths: {[p.name for p in initial_paths]}") + LOGGER.info(f'Initial paths: {[p.name for p in initial_paths]}') nodes = build_nodes(initial_paths, global_config) for node in nodes: @@ -31,8 +34,9 @@ def generate_launch_description(): return ld + def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: - LOGGER.debug("Starting build") + LOGGER.debug('Starting build') # initialize set of processed paths and output list processed = set() node_list = [] @@ -40,15 +44,19 @@ def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: while paths: # capture and error check next path config_path = paths.pop() - LOGGER.info(f"Starting processing on {config_path.as_posix()}") - assert isinstance(config_path, Path), f"Unrecognized build_nodes input type." - if config_path.suffix != ".yaml": - LOGGER.error(f"Non-yaml config file detected: {config_path.as_posix()}. GraphMember configs must be yaml-encoded.\nSkipping...") + LOGGER.info(f'Starting processing on {config_path.as_posix()}') + assert isinstance(config_path, Path), f'Unrecognized build_nodes input type.' + if config_path.suffix != '.yaml': + LOGGER.error( + f'Non-yaml config file detected: {config_path.as_posix()}. GraphMember configs must be yaml-encoded.\nSkipping...' + ) continue if config_path in processed: - LOGGER.error(f"Potential circular path detected in config files.\nConfig file: {config_path.as_posix()} is contained by a sub-member.\nSkipping...") + LOGGER.error( + f'Potential circular path detected in config files.\nConfig file: {config_path.as_posix()} is contained by a sub-member.\nSkipping...' + ) continue - LOGGER.debug(f"non-circular path") + LOGGER.debug(f'non-circular path') # path is checkable, add to processed list processed.add(config_path) @@ -56,65 +64,76 @@ def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: try: abs_path = resolve_config_file(config_path) except FileExistsError: - LOGGER.error(f"Duplicate filenames in Vehicle + Sensor dirs for file: {config_path.as_posix()}.\nSkipping...") + LOGGER.error( + f'Duplicate filenames in Vehicle + Sensor dirs for file: {config_path.as_posix()}.\nSkipping...' + ) continue if abs_path is None: - LOGGER.error(f"Cannot find file: {config_path.as_posix()} in any MAVInsight config folder.\nSkipping...") + LOGGER.error( + f'Cannot find file: {config_path.as_posix()} in any MAVInsight config folder.\nSkipping...' + ) continue - LOGGER.debug(f"abs path acquired") + LOGGER.debug(f'abs path acquired') # open file and confirm yaml encoding - with open(abs_path.as_posix(), "r", encoding="utf-8") as f: + with open(abs_path.as_posix(), 'r', encoding='utf-8') as f: config = yaml.safe_load(f) if type(config) is not dict: - LOGGER.error(f"Error parsing file: {abs_path.as_posix()} as yaml. GraphMember configs must be yaml-encoded.\nSkipping...") + LOGGER.error( + f'Error parsing file: {abs_path.as_posix()} as yaml. GraphMember configs must be yaml-encoded.\nSkipping...' + ) continue - LOGGER.debug(f"file opened successfully") + LOGGER.debug(f'file opened successfully') # parse yaml down to the param layer (remove the layers of nesting above params) while len(config.keys()) == 1: config = config[next(iter(config))] - LOGGER.debug(f"Base yaml acquired") + LOGGER.debug(f'Base yaml acquired') # select the correct executable for this config file try: ex = config['executable'] except KeyError as e: - LOGGER.error(f"Config file: {abs_path.as_posix()} contains no executable param.\nSkipping...") + LOGGER.error( + f'Config file: {abs_path.as_posix()} contains no executable param.\nSkipping...' + ) continue - LOGGER.debug(f"File type identified") + LOGGER.debug(f'File type identified') # create Node action for launch description - node_list.append(Node( - package=package_name, - executable=ex, - name=abs_path.stem, - namespace=namespace, - parameters=[global_config.as_posix(), abs_path.as_posix()], - output="screen", - )) + node_list.append( + Node( + package=package_name, + executable=ex, + name=abs_path.stem, + namespace=namespace, + parameters=[global_config.as_posix(), abs_path.as_posix()], + output='screen', + ) + ) # add sub-members to list of nodes to be built sensors = config.get('sensors', []) if len(sensors) > 0: - LOGGER.info(f"Adding new sensor files: {sensors}") - for sens in config.get("sensors", []): + LOGGER.info(f'Adding new sensor files: {sensors}') + for sens in config.get('sensors', []): paths.append(Path(sens)) vizs = config.get('viz', []) if len(vizs) > 0: - LOGGER.info(f"Adding new visualization files: {vizs}") - for viz in config.get("viz", []): + LOGGER.info(f'Adding new visualization files: {vizs}') + for viz in config.get('viz', []): paths.append(Path(viz)) return node_list + def resolve_config_file(path: Path) -> Path | None: if path.is_absolute(): return path - package_configs = Path(get_package_share_directory(package_name)) / "package_resources" + package_configs = Path(get_package_share_directory(package_name)) / 'package_resources' resolved_path = package_configs / path if not resolved_path.is_file(): - raise FileNotFoundError(f"Could not find configs for: {path} in mavinsight configs folder.") + raise FileNotFoundError(f'Could not find configs for: {path} in mavinsight configs folder.') return resolved_path diff --git a/launch/launch_site.launch.py b/launch/launch_site.launch.py index d5252a2..7db8f70 100644 --- a/launch/launch_site.launch.py +++ b/launch/launch_site.launch.py @@ -1,29 +1,32 @@ # python imports from pathlib import Path + import yaml # ROS imports from ament_index_python import get_package_share_directory -from launch import LaunchDescription, logging from launch_ros.actions import Node -package_name = "mavinsight" -namespace = "viz" -LOGGER = logging.get_logger("vehicle_launch_logger") -initial_paths_overrides = ["c2_c130_maponly.yaml"] +from launch import LaunchDescription, logging + +package_name = 'mavinsight' +namespace = 'viz' +LOGGER = logging.get_logger('vehicle_launch_logger') +initial_paths_overrides = ['c2_c130_maponly.yaml'] + def generate_launch_description(): ld = LaunchDescription() share_dir = Path(get_package_share_directory(package_name)) - shared_resources = share_dir / "package_resources" + shared_resources = share_dir / 'package_resources' global_config = shared_resources / 'global_node_config.yaml' # TODO change behavior for empty initial paths override initial_paths = [(shared_resources) / p for p in initial_paths_overrides] - LOGGER.info(f"Initial paths: {[p.name for p in initial_paths]}") + LOGGER.info(f'Initial paths: {[p.name for p in initial_paths]}') nodes = build_nodes(initial_paths, global_config) for node in nodes: @@ -31,8 +34,9 @@ def generate_launch_description(): return ld + def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: - LOGGER.debug("Starting build") + LOGGER.debug('Starting build') # initialize set of processed paths and output list processed = set() node_list = [] @@ -40,15 +44,19 @@ def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: while paths: # capture and error check next path config_path = paths.pop() - LOGGER.info(f"Starting processing on {config_path.as_posix()}") - assert isinstance(config_path, Path), f"Unrecognized build_nodes input type." - if config_path.suffix != ".yaml": - LOGGER.error(f"Non-yaml config file detected: {config_path.as_posix()}. GraphMember configs must be yaml-encoded.\nSkipping...") + LOGGER.info(f'Starting processing on {config_path.as_posix()}') + assert isinstance(config_path, Path), f'Unrecognized build_nodes input type.' + if config_path.suffix != '.yaml': + LOGGER.error( + f'Non-yaml config file detected: {config_path.as_posix()}. GraphMember configs must be yaml-encoded.\nSkipping...' + ) continue if config_path in processed: - LOGGER.error(f"Potential circular path detected in config files.\nConfig file: {config_path.as_posix()} is contained by a sub-member.\nSkipping...") + LOGGER.error( + f'Potential circular path detected in config files.\nConfig file: {config_path.as_posix()} is contained by a sub-member.\nSkipping...' + ) continue - LOGGER.debug(f"non-circular path") + LOGGER.debug(f'non-circular path') # path is checkable, add to processed list processed.add(config_path) @@ -56,57 +64,68 @@ def build_nodes(paths: list[Path], global_config: Path) -> list[Node]: try: abs_path = resolve_config_file(config_path) except FileExistsError: - LOGGER.error(f"Duplicate filenames in Vehicle + Sensor dirs for file: {config_path.as_posix()}.\nSkipping...") + LOGGER.error( + f'Duplicate filenames in Vehicle + Sensor dirs for file: {config_path.as_posix()}.\nSkipping...' + ) continue if abs_path is None: - LOGGER.error(f"Cannot find file: {config_path.as_posix()} in any MAVInsight config folder.\nSkipping...") + LOGGER.error( + f'Cannot find file: {config_path.as_posix()} in any MAVInsight config folder.\nSkipping...' + ) continue - LOGGER.debug(f"abs path acquired") + LOGGER.debug(f'abs path acquired') # open file and confirm yaml encoding - with open(abs_path.as_posix(), "r", encoding="utf-8") as f: + with open(abs_path.as_posix(), 'r', encoding='utf-8') as f: config = yaml.safe_load(f) if type(config) is not dict: - LOGGER.error(f"Error parsing file: {abs_path.as_posix()} as yaml. GraphMember configs must be yaml-encoded.\nSkipping...") + LOGGER.error( + f'Error parsing file: {abs_path.as_posix()} as yaml. GraphMember configs must be yaml-encoded.\nSkipping...' + ) continue - LOGGER.debug(f"file opened successfully") + LOGGER.debug(f'file opened successfully') # parse yaml down to the param layer (remove the layers of nesting above params) while len(config.keys()) == 1: config = config[next(iter(config))] - LOGGER.debug(f"Base yaml acquired") + LOGGER.debug(f'Base yaml acquired') # select the correct executable for this config file try: ex = config['executable'] except KeyError as e: - LOGGER.error(f"Config file: {abs_path.as_posix()} contains no executable param.\nSkipping...") + LOGGER.error( + f'Config file: {abs_path.as_posix()} contains no executable param.\nSkipping...' + ) continue - LOGGER.debug(f"File type identified") + LOGGER.debug(f'File type identified') # create Node action for launch description - node_list.append(Node( - package=package_name, - executable=ex, - name=abs_path.stem, - namespace=namespace, - parameters=[global_config.as_posix(), abs_path.as_posix()], - output="screen", - )) + node_list.append( + Node( + package=package_name, + executable=ex, + name=abs_path.stem, + namespace=namespace, + parameters=[global_config.as_posix(), abs_path.as_posix()], + output='screen', + ) + ) # add sub-members to list of nodes to be built - LOGGER.info(f"Adding new config files: {config.get('sensors', [])}") - for sens in config.get("sensors", []): + LOGGER.info(f'Adding new config files: {config.get("sensors", [])}') + for sens in config.get('sensors', []): paths.append(Path(sens)) return node_list + def resolve_config_file(path: Path) -> Path | None: if path.is_absolute(): return path - package_configs = Path(get_package_share_directory(package_name)) / "package_resources" + package_configs = Path(get_package_share_directory(package_name)) / 'package_resources' resolved_path = package_configs / path if not resolved_path.is_file(): - raise FileNotFoundError(f"Could not find configs for: {path} in mavinsight configs folder.") + raise FileNotFoundError(f'Could not find configs for: {path} in mavinsight configs folder.') return resolved_path diff --git a/models/frame_member.py b/models/frame_member.py index 6379c32..d0e3e4a 100644 --- a/models/frame_member.py +++ b/models/frame_member.py @@ -1,33 +1,36 @@ # MAVInsight imports from models.graph_member import GraphMember + class FrameMember(GraphMember): FRAME_NAME: str PARENT_FRAME: str - POSE_FRAME: str #TODO: When getting this from px4_msgs, the frame is embedded in the message. get this dynamically from the message? - #TODO: We may need to split this into a position frame and orientation frame (px4 odometry quaternion reported as body->world, but position reported as world->body) + POSE_FRAME: str # TODO: When getting this from px4_msgs, the frame is embedded in the message. get this dynamically from the message? + # TODO: We may need to split this into a position frame and orientation frame (px4 odometry quaternion reported as body->world, but position reported as world->body) def __init__(self): super().__init__() - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Frame Member params....") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Frame Member params....') # Ingest ROS parameters. Notify user when defaults are being used. if self.has_parameter('frame_name'): self.FRAME_NAME = self.get_parameter('frame_name').get_parameter_value().string_value else: - self.default_parameter_warning("frame_name") - self.FRAME_NAME = "base_link" + self.default_parameter_warning('frame_name') + self.FRAME_NAME = 'base_link' if self.has_parameter('parent_frame'): - self.PARENT_FRAME = self.get_parameter('parent_frame').get_parameter_value().string_value + self.PARENT_FRAME = ( + self.get_parameter('parent_frame').get_parameter_value().string_value + ) else: - self.default_parameter_warning("parent_frame") - self.PARENT_FRAME = "map" + self.default_parameter_warning('parent_frame') + self.PARENT_FRAME = 'map' if self.has_parameter('pose_frame'): self.POSE_FRAME = self.get_parameter('pose_frame').get_parameter_value().string_value else: - self.default_parameter_warning("pose_frame") + self.default_parameter_warning('pose_frame') self.POSE_FRAME = 'enu_flu' - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Frame Member initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Frame Member initialized!') diff --git a/models/frame_utils.py b/models/frame_utils.py index dce6d8b..1f02441 100644 --- a/models/frame_utils.py +++ b/models/frame_utils.py @@ -1,33 +1,38 @@ # python imports import numpy as np -from scipy.spatial.transform import Rotation as R import pymap3d as pm # ROS2 message imports from geometry_msgs.msg import Quaternion +from scipy.spatial.transform import Rotation as R from sensor_msgs.msg import NavSatFix # define the NED -> ENU world frame conversion as a remapping of axes -R_ned_enu = R.from_matrix(np.array([ - [0, 1, 0], - [1, 0, 0], - [0, 0,-1], -])) +R_ned_enu = R.from_matrix( + np.array( + [ + [0, 1, 0], + [1, 0, 0], + [0, 0, -1], + ] + ) +) R_enu_ned = R_ned_enu.inv() # define the FRD -> FLU body frame conversions as a remapping of axes (symmetric in this case) -R_frd_flu = R.from_matrix(np.array([ - [1, 0, 0], - [0,-1, 0], - [0, 0,-1] -])) +R_frd_flu = R.from_matrix(np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])) + +R_cam_frd = R.from_matrix( + np.array( + [ + [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + ] + ) +) -R_cam_frd = R.from_matrix(np.array([ - [0, 0, 1], - [1, 0, 0], - [0, 1, 0], -])) def frd_2_flu(input): if isinstance(input, Quaternion): @@ -37,19 +42,23 @@ def frd_2_flu(input): elif isinstance(input, R): return R_frd_flu * input * R_frd_flu else: - raise ValueError(f"Unrecognized input type in frd_2_flu conversion: {input}") + raise ValueError(f'Unrecognized input type in frd_2_flu conversion: {input}') + def frd_ned_2_flu_enu(input): if isinstance(input, Quaternion): r = R.from_quat([input.x, input.y, input.z, input.w]) - (x_out, y_out, z_out, w_out) = ((R_ned_enu * r * R_frd_flu)).as_quat() #NOTE: TF2 quaternions are child->parent + (x_out, y_out, z_out, w_out) = ( + R_ned_enu * r * R_frd_flu + ).as_quat() # NOTE: TF2 quaternions are child->parent return Quaternion(x=x_out, y=y_out, z=z_out, w=w_out) elif isinstance(input, R): return R_frd_flu * input * R_enu_ned else: - raise ValueError(f"Unrecognized input type in frd_ned_2_enu_flu conversion: {input}") + raise ValueError(f'Unrecognized input type in frd_ned_2_enu_flu conversion: {input}') + -def lla_2_enu(reference: NavSatFix, destination: NavSatFix, ignore_alt: bool=True): +def lla_2_enu(reference: NavSatFix, destination: NavSatFix, ignore_alt: bool = True): dest_alt = reference.altitude if ignore_alt else destination.altitude return pm.geodetic2enu( @@ -59,11 +68,9 @@ def lla_2_enu(reference: NavSatFix, destination: NavSatFix, ignore_alt: bool=Tru reference.latitude, reference.longitude, reference.altitude, - deg=True + deg=True, ) + def enu_2_lla(ref: NavSatFix, e, n, u): - return pm.enu2geodetic( - e=e, n=n, u=u, - lat0=ref.latitude, lon0=ref.longitude, h0=ref.altitude - ) + return pm.enu2geodetic(e=e, n=n, u=u, lat0=ref.latitude, lon0=ref.longitude, h0=ref.altitude) diff --git a/models/graph_member.py b/models/graph_member.py index 8cb7f85..4100a93 100644 --- a/models/graph_member.py +++ b/models/graph_member.py @@ -6,6 +6,7 @@ from rclpy.node import Node from tf2_ros import StaticTransformBroadcaster, TransformBroadcaster + class GraphMember(Node): """The base class/Node for all objects that could be displayed in the 3D panel of Foxglove. @@ -29,21 +30,23 @@ class GraphMember(Node): # Constructors def __init__(self): - super().__init__("graph_member", automatically_declare_parameters_from_overrides=True) - self.get_logger().info(f"Received node name: {self.get_name()}") - self.get_logger().info(f"Ingesting Graph Member params...") + super().__init__('graph_member', automatically_declare_parameters_from_overrides=True) + self.get_logger().info(f'Received node name: {self.get_name()}') + self.get_logger().info(f'Ingesting Graph Member params...') # Ingest ROS parameters. Notify user when defaults are being used. if self.has_parameter('display_name'): - self.DISPLAY_NAME = self.get_parameter('display_name').get_parameter_value().string_value + self.DISPLAY_NAME = ( + self.get_parameter('display_name').get_parameter_value().string_value + ) else: - self.default_parameter_warning("display_name") - self.DISPLAY_NAME = "Default Vehicle Name" + self.default_parameter_warning('display_name') + self.DISPLAY_NAME = 'Default Vehicle Name' if self.has_parameter('tab_char'): self._tab_char = self.get_parameter('tab_char').get_parameter_value().string_value else: - self._tab_char = "| " + self._tab_char = '| ' # initialize TF broadcaster self.tf_broadcaster = TransformBroadcaster(self) @@ -52,15 +55,15 @@ def __init__(self): self.tf_static_broadcaster = StaticTransformBroadcaster(self) # Wait for foxglove to subscribe to our topics - self.get_logger().info(f"Waiting for Foxglove...") + self.get_logger().info(f'Waiting for Foxglove...') i = 0 while self.count_subscribers('/tf_static') == 0: - self.get_logger().info(f"...{i*5}sec") - i+=1 + self.get_logger().info(f'...{i * 5}sec') + i += 1 time.sleep(5.0) - self.get_logger().info(f"Foxglove found.") + self.get_logger().info(f'Foxglove found.') - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Graph Member initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Graph Member initialized!') def default_parameter_warning(self, param_name: str): """Helper method to output a boilerplate warning indicating that a default @@ -71,7 +74,7 @@ def default_parameter_warning(self, param_name: str): param_name : str The name of the parameter to be used in the warning. """ - self.get_logger().warn(f"Parameter {param_name} not set in config file. using default") + self.get_logger().warn(f'Parameter {param_name} not set in config file. using default') @classmethod def main(cls, args=None): @@ -82,7 +85,7 @@ def main(cls, args=None): except KeyboardInterrupt: pass finally: - print("Shutting down...") + print('Shutting down...') node.destroy_node() if rclpy.ok(): rclpy.shutdown() diff --git a/models/platforms.py b/models/platforms.py index a407469..5dd33df 100644 --- a/models/platforms.py +++ b/models/platforms.py @@ -1,11 +1,12 @@ from enum import Enum + class Platforms(Enum): """Enum of supported platforms""" - DEFAULT = "default" - FIXED_WING = "fixed_wing" - HEX_COPTER = "hex" - QUAD_COPTER = "quad" - STATIONARY = "stationary" - VTOL = "vtol" + DEFAULT = 'default' + FIXED_WING = 'fixed_wing' + HEX_COPTER = 'hex' + QUAD_COPTER = 'quad' + STATIONARY = 'stationary' + VTOL = 'vtol' diff --git a/models/sensor.py b/models/sensor.py index c6e88f2..019f074 100644 --- a/models/sensor.py +++ b/models/sensor.py @@ -1,23 +1,24 @@ # python imports from __future__ import annotations -import numpy as np -from scipy.spatial.transform import Rotation as R + from typing import Optional # ROS2 message imports import mavros_msgs.msg +import numpy as np import px4_msgs.msg from geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 from nav_msgs.msg import Odometry +from scipy.spatial.transform import Rotation as R from sensor_msgs.msg import Range -from std_msgs.msg import Header # ROS imports from std_msgs.msg import Header +from models.frame_member import FrameMember + # MAVInsight imports from models.frame_utils import frd_2_flu, frd_ned_2_flu_enu -from models.frame_member import FrameMember from models.qos_profiles import viz_qos from models.sensor_types import SensorTypes @@ -27,6 +28,7 @@ FLAGS_PITCH_LOCK = 8 FLAGS_YAW_LOCK = 16 + class Sensor(FrameMember): """Class/Node that defines a sensor and its relation to its parent frame. This class defines how and what information will be published to Foxglove for every Sensor (i.e. @@ -51,51 +53,57 @@ class Sensor(FrameMember): # constructors def __init__(self): super().__init__() - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Sensor params...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Sensor params...') # ingest ROS parameters # notify user when defaults are being used - if self.has_parameter("offset"): - offset_param_val = self.get_parameter("offset").get_parameter_value().double_array_value + if self.has_parameter('offset'): + offset_param_val = self.get_parameter('offset').get_parameter_value().double_array_value try: self.OFFSET = [float(f) for f in offset_param_val] except ValueError as e: self.OFFSET = [] self.get_logger().error( - f"Unable to interpret offset param elements as floats. Using no-offset default.\n" + - f"Received: {offset_param_val}\n" + - f"Error: {e}" + f'Unable to interpret offset param elements as floats. Using no-offset default.\n' + + f'Received: {offset_param_val}\n' + + f'Error: {e}' ) if len(self.OFFSET) != 3: self.OFFSET = [] self.get_logger().error( - f"Offset param must be exactly 3 elements long. Using no-offset default.\n" + - f"Received: {offset_param_val}" + f'Offset param must be exactly 3 elements long. Using no-offset default.\n' + + f'Received: {offset_param_val}' ) else: if sum(self.OFFSET) == 0.0: self.OFFSET = [] else: - self.default_parameter_warning("offset") + self.default_parameter_warning('offset') self.OFFSET = [] - if self.has_parameter("sensor_type"): - self.SENSOR_TYPE = SensorTypes(self.get_parameter("sensor_type").get_parameter_value().string_value) + if self.has_parameter('sensor_type'): + self.SENSOR_TYPE = SensorTypes( + self.get_parameter('sensor_type').get_parameter_value().string_value + ) else: - self.default_parameter_warning("sensor_type") + self.default_parameter_warning('sensor_type') self.SENSOR_TYPE = SensorTypes.DEFAULT - if self.has_parameter("sensors"): - self.SENSORS = list(self.get_parameter("sensors").get_parameter_value().string_array_value) + if self.has_parameter('sensors'): + self.SENSORS = list( + self.get_parameter('sensors').get_parameter_value().string_array_value + ) else: self.SENSORS = [] # broadcast the static transform of an offset, if one is present if len(self.OFFSET) == 3: - static_frame_name = f"{self.FRAME_NAME}_offset" - self.get_logger().info(f"Received valid [x,y,z] sensor offset: {self.OFFSET}m. Building new static TF with child frame: {static_frame_name}") + static_frame_name = f'{self.FRAME_NAME}_offset' + self.get_logger().info( + f'Received valid [x,y,z] sensor offset: {self.OFFSET}m. Building new static TF with child frame: {static_frame_name}' + ) # header head_out = Header(stamp=self.get_clock().now().to_msg(), frame_id=self.PARENT_FRAME) @@ -104,34 +112,39 @@ def __init__(self): # assumed no static rotational offset, for now. TODO pos_out = Vector3(x=self.OFFSET[0], y=self.OFFSET[1], z=self.OFFSET[2]) tf_out = Transform(translation=pos_out) - static_frame_name = f"{self.FRAME_NAME}_mount" + static_frame_name = f'{self.FRAME_NAME}_mount' # build tf - s_t = TransformStamped(header=head_out, child_frame_id=static_frame_name, transform=tf_out) - self.get_logger().debug(f"broadcasting {self.PARENT_FRAME} to {static_frame_name}:\n{s_t}") + s_t = TransformStamped( + header=head_out, child_frame_id=static_frame_name, transform=tf_out + ) + self.get_logger().debug( + f'broadcasting {self.PARENT_FRAME} to {static_frame_name}:\n{s_t}' + ) self.tf_static_broadcaster.sendTransform(s_t) # allow sub-members to attach to this new offset frame self.PARENT_FRAME = static_frame_name - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Sensor initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Sensor initialized!') - def _format(self, tab_depth: int = 0, extra_fields: str = "") -> str: + def _format(self, tab_depth: int = 0, extra_fields: str = '') -> str: t1 = self._tab_char * tab_depth t2 = t1 + self._tab_char - sensors_string = "[]" if len(self.SENSORS) == 0 else "\n" + sensors_string = '[]' if len(self.SENSORS) == 0 else '\n' return ( - f"{t1}{self.DISPLAY_NAME} | Sensor {self.SENSOR_TYPE.name}\n" + - f"{t2}Transform: {self.PARENT_FRAME} -> {self.FRAME_NAME}\n" + - f"{t2}Static offset from parent: (x: {self.OFFSET[0]}, y: {self.OFFSET[1]}, z: {self.OFFSET[2]})\n" + - extra_fields + - f"{t2}Sensors: {sensors_string}" + - "\n".join(t2 + self._tab_char + s for s in self.SENSORS) + f'{t1}{self.DISPLAY_NAME} | Sensor {self.SENSOR_TYPE.name}\n' + + f'{t2}Transform: {self.PARENT_FRAME} -> {self.FRAME_NAME}\n' + + f'{t2}Static offset from parent: (x: {self.OFFSET[0]}, y: {self.OFFSET[1]}, z: {self.OFFSET[2]})\n' + + extra_fields + + f'{t2}Sensors: {sensors_string}' + + '\n'.join(t2 + self._tab_char + s for s in self.SENSORS) ) def __str__(self): return self._format() + class Camera(Sensor): """Class/Node defining a Camera and how/what visualization info will be published for Foxglove. @@ -147,26 +160,29 @@ class Camera(Sensor): # constructors def __init__(self): super().__init__() - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Camera params...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Camera params...') # ingest ROS parameters # notify user when defaults are being used - if self.has_parameter("cam_info_topic"): - self.CAM_INFO_TOPIC = self.get_parameter("cam_info_topic").get_parameter_value().string_value + if self.has_parameter('cam_info_topic'): + self.CAM_INFO_TOPIC = ( + self.get_parameter('cam_info_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("cam_info_topic") - self.CAM_INFO_TOPIC = "camera_info" + self.default_parameter_warning('cam_info_topic') + self.CAM_INFO_TOPIC = 'camera_info' - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Camera initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Camera initialized!') - def _format(self, tab_depth: int = 0, extra_fields: str = "") -> str: + def _format(self, tab_depth: int = 0, extra_fields: str = '') -> str: t = self._tab_char * (tab_depth + 1) - camera_fields = f"{t}Camera info topic: {self.CAM_INFO_TOPIC}\n" + extra_fields + camera_fields = f'{t}Camera info topic: {self.CAM_INFO_TOPIC}\n' + extra_fields return super()._format(tab_depth=tab_depth, extra_fields=camera_fields) def __str__(self): return self._format() + class Gimbal(Sensor): """Class/Node defining a Gimbal and how/what visualization info will be published for Foxglove. @@ -190,7 +206,7 @@ class Gimbal(Sensor): # constructors def __init__(self): super().__init__() - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Camera params...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Camera params...') # initialize gimbal state variables self.retract_commanded = False @@ -200,60 +216,81 @@ def __init__(self): self.yaw_lock_commanded = False # initialize common gimbal variables - self.GIMBAL_REF_FRAME_NAME = f"{self.FRAME_NAME}_ref" + self.GIMBAL_REF_FRAME_NAME = f'{self.FRAME_NAME}_ref' # ingest ROS parameters # notify user when defaults are being used - if self.has_parameter("msg_schema"): - self.msg_schema = self.get_parameter("msg_schema").get_parameter_value().string_value.lower() + if self.has_parameter('msg_schema'): + self.msg_schema = ( + self.get_parameter('msg_schema').get_parameter_value().string_value.lower() + ) else: - self.default_parameter_warning("msg_schema") - self.msg_schema = "mavros" - if self.has_parameter("orientation_topic"): - self.ORIENTATION_TOPIC = self.get_parameter("orientation_topic").get_parameter_value().string_value + self.default_parameter_warning('msg_schema') + self.msg_schema = 'mavros' + if self.has_parameter('orientation_topic'): + self.ORIENTATION_TOPIC = ( + self.get_parameter('orientation_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("orientation_topic") - self.ORIENTATION_TOPIC = "gimbal_orientation" # TODO: Decide on sensible defaults for the position and orientation topic names - if self.has_parameter("command_topic"): - self.COMMAND_TOPIC = self.get_parameter("command_topic").get_parameter_value().string_value + self.default_parameter_warning('orientation_topic') + self.ORIENTATION_TOPIC = 'gimbal_orientation' # TODO: Decide on sensible defaults for the position and orientation topic names + if self.has_parameter('command_topic'): + self.COMMAND_TOPIC = ( + self.get_parameter('command_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("command_topic") - self.COMMAND_TOPIC = "command_topic" + self.default_parameter_warning('command_topic') + self.COMMAND_TOPIC = 'command_topic' # body orientation topic TODO: Rename. Gimbal may not always be mounted to body... - if self.has_parameter("body_orientation_topic"): - self.BODY_TOPIC = self.get_parameter("body_orientation_topic").get_parameter_value().string_value + if self.has_parameter('body_orientation_topic'): + self.BODY_TOPIC = ( + self.get_parameter('body_orientation_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("body_orientation_topic") - self.BODY_TOPIC = "body_topic" + self.default_parameter_warning('body_orientation_topic') + self.BODY_TOPIC = 'body_topic' # initialize subscribers match self.msg_schema: - case "px4_msgs": + case 'px4_msgs': attitude_msg_type = px4_msgs.msg.GimbalDeviceAttitudeStatus body_msg_type = px4_msgs.msg.VehicleOdometry - case "mavros": + case 'mavros': attitude_msg_type = mavros_msgs.msg.GimbalDeviceAttitudeStatus body_msg_type = Odometry # only initialize subscriber for attitude command messages for mavros. no px4 message currently supported - self.create_subscription(mavros_msgs.msg.GimbalManagerSetAttitude, self.COMMAND_TOPIC, self.update_commanded_state, viz_qos) + self.create_subscription( + mavros_msgs.msg.GimbalManagerSetAttitude, + self.COMMAND_TOPIC, + self.update_commanded_state, + viz_qos, + ) case _: - raise ValueError(f"Cannot initialize {self.DISPLAY_NAME} Gimbal viz with message schema: {self.msg_schema}.") - self.create_subscription(attitude_msg_type, self.ORIENTATION_TOPIC, self.publish_orientation, viz_qos) + raise ValueError( + f'Cannot initialize {self.DISPLAY_NAME} Gimbal viz with message schema: {self.msg_schema}.' + ) + self.create_subscription( + attitude_msg_type, self.ORIENTATION_TOPIC, self.publish_orientation, viz_qos + ) self.create_subscription(body_msg_type, self.BODY_TOPIC, self.update_body, viz_qos) - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Gimbal initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Gimbal initialized!') - def update_body(self, msg : Odometry | px4_msgs.msg.VehicleOdometry): + def update_body(self, msg: Odometry | px4_msgs.msg.VehicleOdometry): match self.msg_schema: - case "px4_msgs": + case 'px4_msgs': assert isinstance(msg, px4_msgs.msg.VehicleOdometry) - self.body_orientation = frd_ned_2_flu_enu(Quaternion(x=msg.q[1], y=msg.q[2], z=msg.q[3], w=msg.q[0])) #type: ignore - case "mavros": + self.body_orientation = frd_ned_2_flu_enu( + Quaternion(x=msg.q[1], y=msg.q[2], z=msg.q[3], w=msg.q[0]) + ) # type: ignore + case 'mavros': assert isinstance(msg, Odometry) self.body_orientation = msg.pose.pose.orientation case _: - raise ValueError(f"Unable to assess body orientation for {self.DISPLAY_NAME} with schema {self.msg_schema}.") + raise ValueError( + f'Unable to assess body orientation for {self.DISPLAY_NAME} with schema {self.msg_schema}.' + ) # re-compute the gimbal's ref frame TODO: Figure out where the following functionality should live. R_body_ref = R.identity() @@ -273,7 +310,7 @@ def update_body(self, msg : Odometry | px4_msgs.msg.VehicleOdometry): tf = TransformStamped( header=Header(frame_id=self.PARENT_FRAME), child_frame_id=self.GIMBAL_REF_FRAME_NAME, - transform=Transform(rotation=q_body_ref) + transform=Transform(rotation=q_body_ref), ) self.tf_broadcaster.sendTransform(tf) @@ -302,21 +339,21 @@ def update_body(self, msg : Odometry | px4_msgs.msg.VehicleOdometry): # else: # self.get_logger().warn(f"Unrecognized coordinate frame: {self.COORD_FRAME_TF}. Skipping Native Frame creation.") - def publish_orientation(self, msg : mavros_msgs.msg.GimbalDeviceAttitudeStatus): + def publish_orientation(self, msg: mavros_msgs.msg.GimbalDeviceAttitudeStatus): # NOTE: in mavros, GimbalDeviceAttitudeStatus message does NOT reflect commanded flags, only available flags. # enu -> d4_base_link -> gimbal_mount -> gimbal_ref_frame -> gimbal_frame # construct gimbal attitude frame R_ref_g_FRD = R.from_quat([msg.q.x, msg.q.y, msg.q.z, msg.q.w]) R_ref_g = frd_2_flu(R_ref_g_FRD) - (g_x, g_y, g_z, g_w) = R_ref_g.as_quat() # type: ignore + (g_x, g_y, g_z, g_w) = R_ref_g.as_quat() # type: ignore q_ref_g_FLU = Quaternion(x=g_x, y=g_y, z=g_z, w=g_w) # publish gimbal orientation tf gimbal_tf = TransformStamped( - header = Header(stamp=msg.header.stamp, frame_id=self.GIMBAL_REF_FRAME_NAME), - child_frame_id = f"{self.FRAME_NAME}", - transform = Transform(rotation=q_ref_g_FLU) + header=Header(stamp=msg.header.stamp, frame_id=self.GIMBAL_REF_FRAME_NAME), + child_frame_id=f'{self.FRAME_NAME}', + transform=Transform(rotation=q_ref_g_FLU), ) self.tf_broadcaster.sendTransform(gimbal_tf) @@ -332,15 +369,22 @@ def update_commanded_state(self, msg: mavros_msgs.msg.GimbalManagerSetAttitude): # publish commanded attitude. cmd_tf = TransformStamped( - child_frame_id=f"{self.FRAME_NAME}_commanded_attitude", - header = Header(frame_id=self.GIMBAL_REF_FRAME_NAME), - transform = Transform(rotation=frd_2_flu(msg.q)) + child_frame_id=f'{self.FRAME_NAME}_commanded_attitude', + header=Header(frame_id=self.GIMBAL_REF_FRAME_NAME), + transform=Transform(rotation=frd_2_flu(msg.q)), ) self.tf_broadcaster.sendTransform(cmd_tf) def heading_only_frame(self) -> R: - R_world_body = R.from_quat([self.body_orientation.x, self.body_orientation.y, self.body_orientation.z, self.body_orientation.w]) + R_world_body = R.from_quat( + [ + self.body_orientation.x, + self.body_orientation.y, + self.body_orientation.z, + self.body_orientation.w, + ] + ) # find the +x axis of the body (apply the +x vector to the R_world_body frame) heading_vector_enu = R_world_body.apply([1.0, 0.0, 0.0]) # get only the component of this vector in the XY world plane (remove the Z-component of a vector in ENU space) @@ -353,14 +397,15 @@ def heading_only_frame(self) -> R: heading = np.arctan2(heading_vector_enu[1], heading_vector_enu[0]) return R.from_euler('Z', heading) - def _format(self, tab_depth: int = 0, extra_fields: str = "") -> str: + def _format(self, tab_depth: int = 0, extra_fields: str = '') -> str: t = self._tab_char * (tab_depth + 1) - gimbal_fields = f"{t}Orientation topic: {self.ORIENTATION_TOPIC}\n" + extra_fields + gimbal_fields = f'{t}Orientation topic: {self.ORIENTATION_TOPIC}\n' + extra_fields return super()._format(tab_depth=tab_depth, extra_fields=gimbal_fields) def __str__(self): return self._format() + class Rangefinder(Sensor): """Class/Node defining a Rangefinder and how/what visualization info will be published for Foxglove. @@ -376,35 +421,35 @@ class Rangefinder(Sensor): # constructors def __init__(self): super().__init__() - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Rangefinder params...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Rangefinder params...') # ingest ROS parameters # notify user when defaults are being used. - if self.has_parameter("range_topic"): - self.RANGE_TOPIC = self.get_parameter("range_topic").get_parameter_value().string_value + if self.has_parameter('range_topic'): + self.RANGE_TOPIC = self.get_parameter('range_topic').get_parameter_value().string_value else: - self.default_parameter_warning("range_topic") - self.RANGE_TOPIC = "rangefinder" + self.default_parameter_warning('range_topic') + self.RANGE_TOPIC = 'rangefinder' # initialize subscribers self.create_subscription(Range, self.RANGE_TOPIC, self.publish_rangefinder, viz_qos) - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Rangefinder initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Rangefinder initialized!') def publish_rangefinder(self, msg: Range): d = float(msg.range) tf = TransformStamped( - header = Header(stamp=msg.header.stamp, frame_id=self.PARENT_FRAME), - child_frame_id = f"{self.FRAME_NAME}", - transform = Transform(translation=Vector3(x=d)) + header=Header(stamp=msg.header.stamp, frame_id=self.PARENT_FRAME), + child_frame_id=f'{self.FRAME_NAME}', + transform=Transform(translation=Vector3(x=d)), ) self.tf_broadcaster.sendTransform(tf) - def _format(self, tab_depth: int = 0, extra_fields: str = "") -> str: + def _format(self, tab_depth: int = 0, extra_fields: str = '') -> str: t = self._tab_char * (tab_depth + 1) - rangefinder_fields = f"{t}Range topic: {self.RANGE_TOPIC}\n" + extra_fields + rangefinder_fields = f'{t}Range topic: {self.RANGE_TOPIC}\n' + extra_fields return super()._format(tab_depth=tab_depth, extra_fields=rangefinder_fields) def __str__(self): diff --git a/models/sensor_types.py b/models/sensor_types.py index f2e39c3..0334643 100644 --- a/models/sensor_types.py +++ b/models/sensor_types.py @@ -1,9 +1,10 @@ from enum import Enum + class SensorTypes(Enum): """Enum of supported Sensor types""" - CAMERA = "camera" - DEFAULT = "default" - GIMBAL = "gimbal" - RANGEFINDER = "rangefinder" + CAMERA = 'camera' + DEFAULT = 'default' + GIMBAL = 'gimbal' + RANGEFINDER = 'rangefinder' diff --git a/models/site.py b/models/site.py index f36152a..ed1647a 100644 --- a/models/site.py +++ b/models/site.py @@ -13,8 +13,8 @@ from models.graph_member import GraphMember from models.qos_profiles import reliable_qos, viz_qos -class Site(GraphMember): +class Site(GraphMember): GEOFENCE_TOPIC: str GT_TOPIC: str LOCAL_FRAME: str @@ -23,59 +23,71 @@ class Site(GraphMember): def __init__(self): super().__init__() - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Site params...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Site params...') self.LOCAL_FIX = None # geofence - if self.has_parameter("geofence_topic"): - self.GEOFENCE_TOPIC = self.get_parameter("geofence_topic").get_parameter_value().string_value + if self.has_parameter('geofence_topic'): + self.GEOFENCE_TOPIC = ( + self.get_parameter('geofence_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("geofence_topic") - self.GEOFENCE_TOPIC = "/geofence" + self.default_parameter_warning('geofence_topic') + self.GEOFENCE_TOPIC = '/geofence' - if self.has_parameter("geofence"): - self.geofence = self.get_parameter("geofence").get_parameter_value().double_array_value + if self.has_parameter('geofence'): + self.geofence = self.get_parameter('geofence').get_parameter_value().double_array_value else: self.geofence = None # GTs - if self.has_parameter("ground_truth_topic"): - self.GT_TOPIC = self.get_parameter("ground_truth_topic").get_parameter_value().string_value + if self.has_parameter('ground_truth_topic'): + self.GT_TOPIC = ( + self.get_parameter('ground_truth_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("ground_truth_topic") - self.GT_TOPIC = "/ground_truths" + self.default_parameter_warning('ground_truth_topic') + self.GT_TOPIC = '/ground_truths' - if self.has_parameter("ground_truths"): - self.ground_truths = self.get_parameter("ground_truths").get_parameter_value().double_array_value + if self.has_parameter('ground_truths'): + self.ground_truths = ( + self.get_parameter('ground_truths').get_parameter_value().double_array_value + ) else: self.ground_truths = None # local fix - if self.has_parameter("local_fix_topic"): - local_fix_topic = self.get_parameter("local_fix_topic").get_parameter_value().string_value + if self.has_parameter('local_fix_topic'): + local_fix_topic = ( + self.get_parameter('local_fix_topic').get_parameter_value().string_value + ) else: - raise RuntimeError(f"Site Node: {self.DISPLAY_NAME} local fix param not set. Unable to initialize Site node.") + raise RuntimeError( + f'Site Node: {self.DISPLAY_NAME} local fix param not set. Unable to initialize Site node.' + ) - if self.has_parameter("local_frame"): - self.LOCAL_FRAME = self.get_parameter("local_frame").get_parameter_value().string_value + if self.has_parameter('local_frame'): + self.LOCAL_FRAME = self.get_parameter('local_frame').get_parameter_value().string_value else: - raise RuntimeError(f"Site Node: {self.DISPLAY_NAME} local frame not set. Unable to initialize Site node.") + raise RuntimeError( + f'Site Node: {self.DISPLAY_NAME} local frame not set. Unable to initialize Site node.' + ) - if self.has_parameter("name"): - self.NAME = self.get_parameter("name").get_parameter_value().string_value + if self.has_parameter('name'): + self.NAME = self.get_parameter('name').get_parameter_value().string_value else: - self.default_parameter_warning("name") - self.NAME = "site" + self.default_parameter_warning('name') + self.NAME = 'site' self.timer = self.create_timer(3, self.site_foxglove_loiter) self.create_subscription(NavSatFix, local_fix_topic, self.update_local_fix, viz_qos) self.geofence_pub = self.create_publisher(Marker, self.GEOFENCE_TOPIC, reliable_qos) - self.gt_pub = self.create_publisher(Marker, f"/{self.NAME}{self.GT_TOPIC}", reliable_qos) + self.gt_pub = self.create_publisher(Marker, f'/{self.NAME}{self.GT_TOPIC}', reliable_qos) - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Site Initialized...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Site Initialized...') def update_local_fix(self, msg: NavSatFix): self.LOCAL_FIX = msg @@ -87,34 +99,44 @@ def site_foxglove_loiter(self): if self.geofence: if len(self.geofence) % 2 != 0: raise ValueError( - f"Non-even geofence list length. Ensure all lats and lons are paired.\n" + - f"length: {len(self.geofence)}" + f'Non-even geofence list length. Ensure all lats and lons are paired.\n' + + f'length: {len(self.geofence)}' ) vertices = list(zip(self.geofence[0::2], self.geofence[1::2])) # close the loop if vertices[0] != vertices[-1]: vertices.append(vertices[0]) - vertices = [lla_2_enu(self.LOCAL_FIX, NavSatFix(latitude=lat, longitude=lon)) for lat, lon in vertices] + vertices = [ + lla_2_enu(self.LOCAL_FIX, NavSatFix(latitude=lat, longitude=lon)) + for lat, lon in vertices + ] geofence_points = [Point(x=e, y=n, z=u) for e, n, u in vertices] - self.geofence_pub.publish(Marker( - header=Header(frame_id=self.LOCAL_FRAME), - type=Marker.LINE_STRIP, - action=Marker.ADD, - points=geofence_points, - scale=Vector3(x=0.1, y=0.1, z=1.0), - color=ColorRGBA(r=253.0/256.0, g=138.0/256.0, a=1.0) - )) + self.geofence_pub.publish( + Marker( + header=Header(frame_id=self.LOCAL_FRAME), + type=Marker.LINE_STRIP, + action=Marker.ADD, + points=geofence_points, + scale=Vector3(x=0.1, y=0.1, z=1.0), + color=ColorRGBA(r=253.0 / 256.0, g=138.0 / 256.0, a=1.0), + ) + ) if self.ground_truths: gt_coords = list(zip(self.ground_truths[0::2], self.ground_truths[1::2])) - gt_fixes = [lla_2_enu(self.LOCAL_FIX, NavSatFix(latitude=lat, longitude=lon)) for lat, lon in gt_coords] + gt_fixes = [ + lla_2_enu(self.LOCAL_FIX, NavSatFix(latitude=lat, longitude=lon)) + for lat, lon in gt_coords + ] gt_msg_points = [Point(x=e, y=n, z=u) for e, n, u in gt_fixes] - self.gt_pub.publish(Marker( - header=Header(frame_id=self.LOCAL_FRAME), - type=Marker.POINTS, - action=Marker.ADD, - points=gt_msg_points, - scale=Vector3(x=1.0, y=1.0, z=1.0), - color=ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0) - )) + self.gt_pub.publish( + Marker( + header=Header(frame_id=self.LOCAL_FRAME), + type=Marker.POINTS, + action=Marker.ADD, + points=gt_msg_points, + scale=Vector3(x=1.0, y=1.0, z=1.0), + color=ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0), + ) + ) diff --git a/models/tba_viz.py b/models/tba_viz.py index 07b4831..0a26198 100644 --- a/models/tba_viz.py +++ b/models/tba_viz.py @@ -1,11 +1,11 @@ # python imports import math -from scipy.spatial.transform import Rotation as R # ROS2 message imports -from cdcl_umd_msgs.msg import TargetBoxArray, TargetBox +from cdcl_umd_msgs.msg import TargetBox, TargetBoxArray from foxglove_msgs.msg import ImageAnnotations, Point2, PointsAnnotation, TextAnnotation from geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from scipy.spatial.transform import Rotation as R from sensor_msgs.msg import Image, NavSatFix from std_msgs.msg import ColorRGBA, Header from vision_msgs.msg import BoundingBox2D @@ -14,45 +14,61 @@ # MAVInsight imports from models.frame_utils import frd_2_flu, lla_2_enu from models.graph_member import GraphMember -from models.qos_profiles import viz_qos, reliable_qos +from models.qos_profiles import reliable_qos, viz_qos -class TBA_Viz(GraphMember): +class TBA_Viz(GraphMember): def __init__(self): super().__init__() self.LOCAL_FIX = None - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Localization params....") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Localization params....') if self.has_parameter('localization_topic'): loc_topic = self.get_parameter('localization_topic').get_parameter_value().string_value else: - raise RuntimeError(f"Localization viz node: {self.DISPLAY_NAME} localization topic param not set. Unable to initialize localization vizualization.") + raise RuntimeError( + f'Localization viz node: {self.DISPLAY_NAME} localization topic param not set. Unable to initialize localization vizualization.' + ) if self.has_parameter('loc_viz_topic_latest'): - loc_viz_topic_latest = self.get_parameter('loc_viz_topic_latest').get_parameter_value().string_value + loc_viz_topic_latest = ( + self.get_parameter('loc_viz_topic_latest').get_parameter_value().string_value + ) else: self.default_parameter_warning('loc_viz_topic_latest') loc_viz_topic_latest = '/viz/tbas/latest' if self.has_parameter('loc_viz_topic_previous'): - loc_viz_topic_previous = self.get_parameter('loc_viz_topic_previous').get_parameter_value().string_value + loc_viz_topic_previous = ( + self.get_parameter('loc_viz_topic_previous').get_parameter_value().string_value + ) else: self.default_parameter_warning('loc_viz_topic_previous') loc_viz_topic_previous = '/viz/tbas/previous' if self.has_parameter('localization_frame'): - self.LOC_FRAME = self.get_parameter('localization_frame').get_parameter_value().string_value + self.LOC_FRAME = ( + self.get_parameter('localization_frame').get_parameter_value().string_value + ) else: - raise RuntimeError(f"Localization viz node: {self.DISPLAY_NAME} localization frame param not set. Unable to initialize localization vizualization.") + raise RuntimeError( + f'Localization viz node: {self.DISPLAY_NAME} localization frame param not set. Unable to initialize localization vizualization.' + ) if self.has_parameter('local_fix_topic'): - local_fix_topic = self.get_parameter('local_fix_topic').get_parameter_value().string_value + local_fix_topic = ( + self.get_parameter('local_fix_topic').get_parameter_value().string_value + ) else: - raise RuntimeError(f"Localization viz node: {self.DISPLAY_NAME} local fix topic param not set. Unable to initialize localization vizualization.") + raise RuntimeError( + f'Localization viz node: {self.DISPLAY_NAME} local fix topic param not set. Unable to initialize localization vizualization.' + ) if self.has_parameter('target_image_topic'): - image_topic = self.get_parameter('target_image_topic').get_parameter_value().string_value + image_topic = ( + self.get_parameter('target_image_topic').get_parameter_value().string_value + ) else: self.default_parameter_warning('target_image_topic') image_topic = 'loczn_img' @@ -63,9 +79,11 @@ def __init__(self): self.default_parameter_warning('bbox_topic') bbox_topic = 'bboxes' - if self.has_parameter("mesh_resource_path"): - self.mesh_resource_path = self.get_parameter("mesh_resource_path").get_parameter_value().string_value - self.get_logger().info(f"Mesh Resource Path detected: {self.mesh_resource_path}") + if self.has_parameter('mesh_resource_path'): + self.mesh_resource_path = ( + self.get_parameter('mesh_resource_path').get_parameter_value().string_value + ) + self.get_logger().info(f'Mesh Resource Path detected: {self.mesh_resource_path}') else: self.mesh_resource_path = None @@ -77,13 +95,13 @@ def __init__(self): self.bbox_pub = self.create_publisher(ImageAnnotations, bbox_topic, reliable_qos) self.i = 0 - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Localization Visualization initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Localization Visualization initialized!') def update_local_fix(self, msg: NavSatFix): self.LOCAL_FIX = msg def loc_cb(self, msg: TargetBoxArray): - self.get_logger().debug("tba received") + self.get_logger().debug('tba received') self.loczn_img_pub.publish(msg.source_img) @@ -91,24 +109,24 @@ def loc_cb(self, msg: TargetBoxArray): drone_pose = msg.uav_local_pose.pose.pose drone_marker = Marker() - drone_marker.header=Header(frame_id=self.LOC_FRAME) - drone_marker.ns="drone" - drone_marker.id=0 - drone_marker.pose.position.x=drone_pose.position.x - drone_marker.pose.position.y=drone_pose.position.y - drone_marker.pose.position.z=drone_pose.position.z - drone_marker.pose.orientation.x=drone_pose.orientation.x - drone_marker.pose.orientation.y=drone_pose.orientation.y - drone_marker.pose.orientation.z=drone_pose.orientation.z - drone_marker.pose.orientation.w=drone_pose.orientation.w - drone_marker.type=Marker.MESH_RESOURCE - drone_marker.mesh_resource=self.mesh_resource_path - drone_marker.mesh_use_embedded_materials=True - drone_marker.action=Marker.ADD - drone_marker.scale=Vector3(x=0.001, y=0.001, z=0.001) - drone_marker.color=ColorRGBA(r=153.0/255.0, g=153.0/255.0, b=153.0/255.0, a=0.9) - - R_fix = R.from_euler('xyz', [0.0, 0.0, -math.pi/2], degrees=False) + drone_marker.header = Header(frame_id=self.LOC_FRAME) + drone_marker.ns = 'drone' + drone_marker.id = 0 + drone_marker.pose.position.x = drone_pose.position.x + drone_marker.pose.position.y = drone_pose.position.y + drone_marker.pose.position.z = drone_pose.position.z + drone_marker.pose.orientation.x = drone_pose.orientation.x + drone_marker.pose.orientation.y = drone_pose.orientation.y + drone_marker.pose.orientation.z = drone_pose.orientation.z + drone_marker.pose.orientation.w = drone_pose.orientation.w + drone_marker.type = Marker.MESH_RESOURCE + drone_marker.mesh_resource = self.mesh_resource_path + drone_marker.mesh_use_embedded_materials = True + drone_marker.action = Marker.ADD + drone_marker.scale = Vector3(x=0.001, y=0.001, z=0.001) + drone_marker.color = ColorRGBA(r=153.0 / 255.0, g=153.0 / 255.0, b=153.0 / 255.0, a=0.9) + + R_fix = R.from_euler('xyz', [0.0, 0.0, -math.pi / 2], degrees=False) q = drone_marker.pose.orientation R_drone = R.from_quat([q.x, q.y, q.z, q.w]) @@ -120,12 +138,12 @@ def loc_cb(self, msg: TargetBoxArray): drone_q = drone_pose.orientation drone_r = R.from_quat([drone_q.x, drone_q.y, drone_q.z, drone_q.w]) - (dr_x, dr_y, dr_z) = drone_r.as_euler("xyz", degrees=True) + (dr_x, dr_y, dr_z) = drone_r.as_euler('xyz', degrees=True) gimbal_q_frd = msg.gimbal_attitude_quaternion gimbal_r_frd = R.from_quat([gimbal_q_frd.x, gimbal_q_frd.y, gimbal_q_frd.z, gimbal_q_frd.w]) gimbal_r_enu = frd_2_flu(gimbal_r_frd) - assert(isinstance(gimbal_r_enu, R)) + assert isinstance(gimbal_r_enu, R) (gr_x, gr_y, gr_z) = gimbal_r_enu.as_euler('xyz', degrees=True) R_world_gimbal = R.from_euler('yz', [float(gr_y), float(dr_z)], degrees=True) @@ -133,16 +151,13 @@ def loc_cb(self, msg: TargetBoxArray): rangefinder_marker = Marker( header=Header(frame_id=self.LOC_FRAME), - ns="rangefinder", + ns='rangefinder', id=0, type=Marker.ARROW, action=Marker.ADD, - pose = Pose( - position=drone_pose.position, - orientation=Quaternion(x=x, y=y, z=z, w=w) - ), + pose=Pose(position=drone_pose.position, orientation=Quaternion(x=x, y=y, z=z, w=w)), scale=Vector3(x=msg.rangefinder_dist.range, y=0.1, z=0.1), - color=ColorRGBA(r=86.0/255.0, g=209.0/255.0, b=86.0/255.0, a=0.75) + color=ColorRGBA(r=86.0 / 255.0, g=209.0 / 255.0, b=86.0 / 255.0, a=0.75), ) markers = [drone_marker, rangefinder_marker] @@ -153,7 +168,7 @@ def loc_cb(self, msg: TargetBoxArray): altimeter_plane_fixes = [] for box in msg.uav_target_boxes: - assert(isinstance(box, TargetBox)) + assert isinstance(box, TargetBox) if box.target_location_altimeter_plane: loc: NavSatFix = box.target_location_altimeter_plane altimeter_plane_fixes.append(lla_2_enu(self.LOCAL_FIX, loc)) @@ -170,62 +185,72 @@ def loc_cb(self, msg: TargetBoxArray): gimbal_plane_points = [Point(x=e, y=n, z=u) for e, n, u in gimbal_plane_fixes] altimeter_plane_points = [Point(x=e, y=n, z=u) for e, n, u in altimeter_plane_fixes] - markers.append(Marker( - header=Header(frame_id=self.LOC_FRAME), - ns="range_last", - id=0, - type=Marker.SPHERE_LIST, - action=Marker.ADD, - points=rangefinder_points, - scale=Vector3(x=0.25, y=0.25, z=0.25), - color=ColorRGBA(r=255.0/255.0, g=0.0/255.0, b=0.0/255.0, a=0.75) - )) - - markers.append(Marker( - header=Header(frame_id=self.LOC_FRAME), - ns="gimb_plane_last", - id=0, - type=Marker.SPHERE_LIST, - action=Marker.ADD, - points=gimbal_plane_points, - scale=Vector3(x=0.25, y=0.25, z=0.25), - color=ColorRGBA(r=0.0/255.0, g=255.0/255.0, b=0.0/255.0, a=0.75) - )) - - markers.append(Marker( - header=Header(frame_id=self.LOC_FRAME), - ns="alt_plane_last", - id=0, - type=Marker.SPHERE_LIST, - action=Marker.ADD, - points=altimeter_plane_points, - scale=Vector3(x=0.25, y=0.25, z=0.25), - color=ColorRGBA(r=0.0/255.0, g=0.0/255.0, b=255.0/255.0, a=0.75) - )) + markers.append( + Marker( + header=Header(frame_id=self.LOC_FRAME), + ns='range_last', + id=0, + type=Marker.SPHERE_LIST, + action=Marker.ADD, + points=rangefinder_points, + scale=Vector3(x=0.25, y=0.25, z=0.25), + color=ColorRGBA(r=255.0 / 255.0, g=0.0 / 255.0, b=0.0 / 255.0, a=0.75), + ) + ) + + markers.append( + Marker( + header=Header(frame_id=self.LOC_FRAME), + ns='gimb_plane_last', + id=0, + type=Marker.SPHERE_LIST, + action=Marker.ADD, + points=gimbal_plane_points, + scale=Vector3(x=0.25, y=0.25, z=0.25), + color=ColorRGBA(r=0.0 / 255.0, g=255.0 / 255.0, b=0.0 / 255.0, a=0.75), + ) + ) + + markers.append( + Marker( + header=Header(frame_id=self.LOC_FRAME), + ns='alt_plane_last', + id=0, + type=Marker.SPHERE_LIST, + action=Marker.ADD, + points=altimeter_plane_points, + scale=Vector3(x=0.25, y=0.25, z=0.25), + color=ColorRGBA(r=0.0 / 255.0, g=0.0 / 255.0, b=255.0 / 255.0, a=0.75), + ) + ) altimeter_beam_points = [] - drone_point = Point(x=drone_pose.position.x, y=drone_pose.position.y, z=drone_pose.position.z) + drone_point = Point( + x=drone_pose.position.x, y=drone_pose.position.y, z=drone_pose.position.z + ) for p in altimeter_plane_points: altimeter_beam_points.append(drone_point) altimeter_beam_points.append(p) - markers.append(Marker( - header=Header(frame_id=self.LOC_FRAME), - ns="alt_beams_last", - id=0, - type=Marker.LINE_LIST, - action=Marker.ADD, - points=altimeter_beam_points, - scale=Vector3(x=0.05, y=0.05, z=0.05), - color=ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0) - )) + markers.append( + Marker( + header=Header(frame_id=self.LOC_FRAME), + ns='alt_beams_last', + id=0, + type=Marker.LINE_LIST, + action=Marker.ADD, + points=altimeter_beam_points, + scale=Vector3(x=0.05, y=0.05, z=0.05), + color=ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0), + ) + ) self.latest_pub.publish(MarkerArray(markers=markers)) [latest_to_previous(m, self.i) for m in markers] self.previous_pub.publish(MarkerArray(markers=markers)) - self.i+=1 + self.i += 1 def generage_bboxes(self, msg: TargetBoxArray): out = ImageAnnotations() @@ -233,7 +258,7 @@ def generage_bboxes(self, msg: TargetBoxArray): out.texts = [] for i, tb in enumerate(msg.uav_target_boxes): - assert(isinstance(tb, TargetBox)) + assert isinstance(tb, TargetBox) ann = PointsAnnotation() ann.timestamp = msg.header.stamp ann.type = PointsAnnotation.LINE_LOOP @@ -248,7 +273,7 @@ def generage_bboxes(self, msg: TargetBoxArray): ann.fill_color.b = 0.0 ann.fill_color.a = 0.12 - box:BoundingBox2D = tb.target_bbox + box: BoundingBox2D = tb.target_bbox cx = box.center.position.x cy = box.center.position.y th = box.center.theta @@ -262,14 +287,16 @@ def generage_bboxes(self, msg: TargetBoxArray): (cx - hx, cy - hy), (cx + hx, cy - hy), (cx + hx, cy + hy), - (cx - hx, cy + hy) + (cx - hx, cy + hy), ] ann.points = [] for x, y in points: dx = x - cx dy = y - cy - ann.points.append(Point2(x=float(cx + ct*dx - st*dy), y=float(cy + st*dx + ct*dy))) + ann.points.append( + Point2(x=float(cx + ct * dx - st * dy), y=float(cy + st * dx + ct * dy)) + ) out.points.append(ann) @@ -291,7 +318,8 @@ def generage_bboxes(self, msg: TargetBoxArray): self.bbox_pub.publish(out) -def latest_to_previous(input:Marker, i: int): + +def latest_to_previous(input: Marker, i: int): input.id = i - input.ns = input.ns.replace("last", "all") + input.ns = input.ns.replace('last', 'all') return input diff --git a/models/vehicle.py b/models/vehicle.py index 9bb569f..04ddeef 100644 --- a/models/vehicle.py +++ b/models/vehicle.py @@ -1,10 +1,10 @@ # python imports from __future__ import annotations -# ROS2 message imports from math import isclose from typing import Optional, Tuple +# ROS2 message imports from geometry_msgs.msg import ( Point, PoseStamped, @@ -21,12 +21,14 @@ from std_msgs.msg import Header from visualization_msgs.msg import Marker +from models.frame_member import FrameMember + # MAVInsight imports from models.frame_utils import enu_2_lla, frd_ned_2_flu_enu -from models.frame_member import FrameMember from models.platforms import Platforms from models.qos_profiles import reliable_qos, viz_qos + class Vehicle(FrameMember): """Class/Node that defines a generic vehicle (typically a drone) and its sensors. This Class defines what information should be published for all Vehicles. (i.e. @@ -50,133 +52,141 @@ class Vehicle(FrameMember): def __init__(self): super().__init__() self.latest_header = None - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Ingesting Vehicle params...") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Ingesting Vehicle params...') # ingest ROS parameters. Notify user when defaults are being used - if self.has_parameter("altitude_topic"): - alt_topic = self.get_parameter("altitude_topic").get_parameter_value().string_value + if self.has_parameter('altitude_topic'): + alt_topic = self.get_parameter('altitude_topic').get_parameter_value().string_value else: self.default_parameter_warning('altitude_topic') - alt_topic = "/altitude" + alt_topic = '/altitude' # Global Refresh Rate - if self.has_parameter("refresh_rate"): + if self.has_parameter('refresh_rate'): self.REFRESH_RATE = ( - self.get_parameter("refresh_rate").get_parameter_value().double_value + self.get_parameter('refresh_rate').get_parameter_value().double_value ) else: - self.default_parameter_warning("refresh_rate") + self.default_parameter_warning('refresh_rate') self.REFRESH_RATE = 60.0 # Hz # Namespace - if self.has_parameter("namespace"): - namespace = ( - self.get_parameter("namespace").get_parameter_value().string_value - ) + if self.has_parameter('namespace'): + namespace = self.get_parameter('namespace').get_parameter_value().string_value else: - self.default_parameter_warning("namespace") - namespace = "/uas/" + self.default_parameter_warning('namespace') + namespace = '/uas/' # ekf origin - if self.has_parameter("ekf_origin_fix_topic"): - ekf_topic = self.get_parameter("ekf_origin_fix_topic").get_parameter_value().string_value + if self.has_parameter('ekf_origin_fix_topic'): + ekf_topic = ( + self.get_parameter('ekf_origin_fix_topic').get_parameter_value().string_value + ) else: - raise RuntimeError(f"Vehicle Node: {self.DISPLAY_NAME} ekf origin fix topic param not set. Unable to initialize Vehicle node.") + raise RuntimeError( + f'Vehicle Node: {self.DISPLAY_NAME} ekf origin fix topic param not set. Unable to initialize Vehicle node.' + ) - if self.has_parameter("ekf_origin_frame"): - self.EKF_FRAME = self.get_parameter("ekf_origin_frame").get_parameter_value().string_value + if self.has_parameter('ekf_origin_frame'): + self.EKF_FRAME = ( + self.get_parameter('ekf_origin_frame').get_parameter_value().string_value + ) else: - raise RuntimeError(f"Vehicle Node: {self.DISPLAY_NAME} ekf origin frame param not set. Unable to initialize Vehicle node.") + raise RuntimeError( + f'Vehicle Node: {self.DISPLAY_NAME} ekf origin frame param not set. Unable to initialize Vehicle node.' + ) # Home Position - if self.has_parameter("home_position_topic"): - home_pos_topic = self.get_parameter("home_position_topic").get_parameter_value().string_value + if self.has_parameter('home_position_topic'): + home_pos_topic = ( + self.get_parameter('home_position_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("home_position_topic") - home_pos_topic = "/home_position/home" + self.default_parameter_warning('home_position_topic') + home_pos_topic = '/home_position/home' - if self.has_parameter("home_fix_topic"): - home_fix_topic = self.get_parameter("home_fix_topic").get_parameter_value().string_value + if self.has_parameter('home_fix_topic'): + home_fix_topic = self.get_parameter('home_fix_topic').get_parameter_value().string_value else: - self.default_parameter_warning("home_fix_topic") - home_fix_topic = "/home_position/fix" + self.default_parameter_warning('home_fix_topic') + home_fix_topic = '/home_position/fix' - if self.has_parameter("home_frame_topic"): - self.HOME_FRAME = self.get_parameter("home_frame_topic").get_parameter_value().string_value + if self.has_parameter('home_frame_topic'): + self.HOME_FRAME = ( + self.get_parameter('home_frame_topic').get_parameter_value().string_value + ) else: - self.default_parameter_warning("home_frame_topic") - self.HOME_FRAME = "home_position" + self.default_parameter_warning('home_frame_topic') + self.HOME_FRAME = 'home_position' # Location Topic - if self.has_parameter("location_topic"): + if self.has_parameter('location_topic'): self.LOCATION_TOPIC = ( - self.get_parameter("location_topic").get_parameter_value().string_value + self.get_parameter('location_topic').get_parameter_value().string_value ) else: - self.default_parameter_warning("location_topic") - self.LOCATION_TOPIC = "gps" + self.default_parameter_warning('location_topic') + self.LOCATION_TOPIC = 'gps' - if self.has_parameter("velocity_topic"): - velocity_topic = self.get_parameter("velocity_topic").get_parameter_value().string_value + if self.has_parameter('velocity_topic'): + velocity_topic = self.get_parameter('velocity_topic').get_parameter_value().string_value else: - self.default_parameter_warning("velocity_topic") - velocity_topic = "vel" + self.default_parameter_warning('velocity_topic') + velocity_topic = 'vel' # Platform Type - if self.has_parameter("platform"): + if self.has_parameter('platform'): self.PLATFORM = Platforms( - self.get_parameter("platform").get_parameter_value().string_value + self.get_parameter('platform').get_parameter_value().string_value ) else: - self.default_parameter_warning("platform") + self.default_parameter_warning('platform') self.PLATFORM = Platforms.DEFAULT # Sensors - if self.has_parameter("sensors"): + if self.has_parameter('sensors'): self.SENSORS = list( - self.get_parameter("sensors").get_parameter_value().string_array_value + self.get_parameter('sensors').get_parameter_value().string_array_value ) else: self.SENSORS = [] # Position tolerance for path de-duplication - if self.has_parameter("position_tolerance"): - self.POSITION_TOLERANCE = float( - self.get_parameter("position_tolerance").value - ) + if self.has_parameter('position_tolerance'): + self.POSITION_TOLERANCE = float(self.get_parameter('position_tolerance').value) else: - self.default_parameter_warning("position_tolerance") + self.default_parameter_warning('position_tolerance') self.POSITION_TOLERANCE = 0.0254 # 1 inch in meters # Message Schema - if self.has_parameter("message_schema"): - msg_schema_str = ( - self.get_parameter("message_schema").get_parameter_value().string_value - ) - if msg_schema_str.lower() == "px4_msgs": + if self.has_parameter('message_schema'): + msg_schema_str = self.get_parameter('message_schema').get_parameter_value().string_value + if msg_schema_str.lower() == 'px4_msgs': self.LOCATION_MSG_TYPE = VehicleOdometry else: self.LOCATION_MSG_TYPE = PoseStamped else: - self.default_parameter_warning("message_schema") + self.default_parameter_warning('message_schema') self.LOCATION_MSG_TYPE = PoseStamped # Initialize subscribers self.create_subscription(Altitude, alt_topic, self.update_alt, viz_qos) - self.create_subscription(self.LOCATION_MSG_TYPE, self.LOCATION_TOPIC, self.publish_position, viz_qos) + self.create_subscription( + self.LOCATION_MSG_TYPE, self.LOCATION_TOPIC, self.publish_position, viz_qos + ) self.create_subscription(HomePosition, home_pos_topic, self.home_cb, viz_qos) self.create_subscription(TwistStamped, velocity_topic, self.update_velocity, viz_qos) self.ALTITUDE = None self.VELOCITY = None # Initialize publishers - self.path_pub = self.create_publisher(Path, f"{namespace}flightPath", reliable_qos) + self.path_pub = self.create_publisher(Path, f'{namespace}flightPath', reliable_qos) self.home_fix_pub = self.create_publisher(NavSatFix, home_fix_topic, reliable_qos) self.ekf_fix_pub = self.create_publisher(NavSatFix, ekf_topic, reliable_qos) # Publisher for velocity vector visualization markers self.velocity_vector_marker_pub = self.create_publisher( - Marker, f"{namespace}velocityVector", reliable_qos + Marker, f'{namespace}velocityVector', reliable_qos ) # Internal storage for path visualizer @@ -188,7 +198,7 @@ def __init__(self): self.drone_velocity = [0.0, 0.0, 0.0] # Current velocity (m/s) self.drone_pos = [0.0, 0.0, 0.0] # Current position (m) self.last_drone_pos: Optional[Tuple[float, float, float]] = ( - None # Last position reading + None # Last published position for path de-duplication ) self.target_velocity = [0.0, 0.0, 0.0] # Target velocity (m/s) self.target_pos = [0.0, 0.0, 0.0] # Target position (m) @@ -197,20 +207,18 @@ def __init__(self): self.create_timer(1.0 / self.REFRESH_RATE, self.publish_path) self.create_timer(1.0 / self.REFRESH_RATE, self.publish_velocity_vector) - self.get_logger().info(f"[{self.DISPLAY_NAME}]: Vehicle initialized!") + self.get_logger().info(f'[{self.DISPLAY_NAME}]: Vehicle initialized!') def update_alt(self, msg: Altitude): - self.ALTITUDE=msg + self.ALTITUDE = msg def update_velocity(self, msg: TwistStamped): - self.VELOCITY=msg + self.VELOCITY = msg def publish_position(self, msg: PoseStamped | VehicleOdometry): # header # TODO: double check time sync between message schemas - head_out = Header( - stamp=self.get_clock().now().to_msg(), frame_id=self.PARENT_FRAME - ) + head_out = Header(stamp=self.get_clock().now().to_msg(), frame_id=self.PARENT_FRAME) path_update = PoseStamped() @@ -267,15 +275,12 @@ def publish_position(self, msg: PoseStamped | VehicleOdometry): self.drone_pos = list(new_pos) path_update.header = head_out - self.path.header.stamp = path_update.header.stamp # keep the most recent header for downstream publishers self.latest_header = head_out # build TF - t = TransformStamped( - header=head_out, child_frame_id=self.FRAME_NAME, transform=tf_out - ) + t = TransformStamped(header=head_out, child_frame_id=self.FRAME_NAME, transform=tf_out) self.tf_broadcaster.sendTransform(t) @@ -286,6 +291,7 @@ def publish_position(self, msg: PoseStamped | VehicleOdometry): ): self.path.poses.append(path_update) # type: ignore self.last_drone_pos = new_pos + self.path.header.stamp = path_update.header.stamp # Publish altimeter plane if self.ALTITUDE: @@ -295,35 +301,41 @@ def publish_position(self, msg: PoseStamped | VehicleOdometry): # altitude.relative seems to publish the drone height relative to the home position (not sure why this is inconsistent with reality) # altitude.bottom_clearance : I'm assuming this would be consistent with the plane that the drone is hovering over. alt_t.translation.z -= self.ALTITUDE.bottom_clearance - self.tf_broadcaster.sendTransform(TransformStamped( - header=head_out, - child_frame_id=f"{self.FRAME_NAME}_alt_plane", - transform=alt_t - )) - + self.tf_broadcaster.sendTransform( + TransformStamped( + header=head_out, child_frame_id=f'{self.FRAME_NAME}_alt_plane', transform=alt_t + ) + ) def home_cb(self, msg: HomePosition): home_fix = NavSatFix( header=Header(frame_id=self.HOME_FRAME), latitude=msg.geo.latitude, longitude=msg.geo.longitude, - altitude=msg.geo.altitude + altitude=msg.geo.altitude, ) self.home_fix_pub.publish(home_fix) - self.tf_broadcaster.sendTransform(TransformStamped( - header=Header(stamp=self.get_clock().now().to_msg(), frame_id=self.HOME_FRAME), - child_frame_id=self.EKF_FRAME, - transform=Transform(translation=Vector3(x=-msg.position.x, y=-msg.position.y, z=-msg.position.z)) - )) - - (lat_e, lon_e, alt_e) = enu_2_lla(home_fix, -msg.position.x, -msg.position.y, -msg.position.z) - self.ekf_fix_pub.publish(NavSatFix( - header=Header(frame_id=self.EKF_FRAME), - latitude=lat_e, - longitude=lon_e, - altitude=alt_e - )) + self.tf_broadcaster.sendTransform( + TransformStamped( + header=Header(stamp=self.get_clock().now().to_msg(), frame_id=self.HOME_FRAME), + child_frame_id=self.EKF_FRAME, + transform=Transform( + translation=Vector3(x=-msg.position.x, y=-msg.position.y, z=-msg.position.z) + ), + ) + ) + (lat_e, lon_e, alt_e) = enu_2_lla( + home_fix, -msg.position.x, -msg.position.y, -msg.position.z + ) + self.ekf_fix_pub.publish( + NavSatFix( + header=Header(frame_id=self.EKF_FRAME), + latitude=lat_e, + longitude=lon_e, + altitude=alt_e, + ) + ) def publish_path(self): if self.path.poses: @@ -345,14 +357,12 @@ def publish_velocity_vector(self): velocity_vector_marker = Marker() velocity_vector_marker.header.stamp = stamp velocity_vector_marker.header.frame_id = self.PARENT_FRAME - velocity_vector_marker.ns = "velocity_vector" + velocity_vector_marker.ns = 'velocity_vector' velocity_vector_marker.id = 0 velocity_vector_marker.type = Marker.ARROW velocity_vector_marker.action = Marker.ADD - start_point = Point( - x=self.drone_pos[0], y=self.drone_pos[1], z=self.drone_pos[2] - ) + start_point = Point(x=self.drone_pos[0], y=self.drone_pos[1], z=self.drone_pos[2]) end_point = Point(x=target_pos[0], y=target_pos[1], z=target_pos[2]) velocity_vector_marker.points = [start_point, end_point] @@ -367,13 +377,13 @@ def publish_velocity_vector(self): self.velocity_vector_marker_pub.publish(velocity_vector_marker) def position_conversion(self, x_in: float, y_in: float, z_in: float) -> Vector3: - if "ned" in self.POSE_FRAME: + if 'ned' in self.POSE_FRAME: return Vector3(x=y_in, y=x_in, z=-z_in) - elif "enu" in self.POSE_FRAME: + elif 'enu' in self.POSE_FRAME: return Vector3(x=x_in, y=y_in, z=z_in) else: raise ValueError( - f"Unable to determine the coordinate frame for message type: {self.POSE_FRAME}" + f'Unable to determine the coordinate frame for message type: {self.POSE_FRAME}' ) @staticmethod @@ -385,14 +395,14 @@ def _positions_equal( def _format(self, tab_depth: int = 0) -> str: t1 = self._tab_char * tab_depth t2 = t1 + self._tab_char - sensors_string = "[]" if len(self.SENSORS) == 0 else "\n" + sensors_string = '[]' if len(self.SENSORS) == 0 else '\n' return ( - f"Vehicle Structure ({self.get_name()}):\n" - + f"{t1}{self.DISPLAY_NAME} | Vehicle ({self.PLATFORM.name})\n" - + f"{t2}Transform: {self.PARENT_FRAME} -> {self.FRAME_NAME}\n" - + f"{t2}Location Topic: {self.LOCATION_TOPIC}\n" - + f"{t2}Sensors: {sensors_string}" - + ("\n".join(t2 + self._tab_char + s for s in self.SENSORS)) + f'Vehicle Structure ({self.get_name()}):\n' + + f'{t1}{self.DISPLAY_NAME} | Vehicle ({self.PLATFORM.name})\n' + + f'{t2}Transform: {self.PARENT_FRAME} -> {self.FRAME_NAME}\n' + + f'{t2}Location Topic: {self.LOCATION_TOPIC}\n' + + f'{t2}Sensors: {sensors_string}' + + ('\n'.join(t2 + self._tab_char + s for s in self.SENSORS)) ) def __str__(self): diff --git a/pyproject.toml b/pyproject.toml index 9787c3b..027940a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,3 +1,22 @@ [build-system] requires = ["setuptools", "wheel"] build-backend = "setuptools.build_meta" + +[tool.ruff] +line-length = 100 +target-version = "py310" +extend-exclude = [ + "build", + "install", + "log", +] + +[tool.ruff.format] +quote-style = "single" +indent-style = "space" +line-ending = "lf" + +[tool.ruff.lint] +select = [ + "I", +] diff --git a/requirements-dev.txt b/requirements-dev.txt new file mode 100644 index 0000000..80f44b0 --- /dev/null +++ b/requirements-dev.txt @@ -0,0 +1 @@ +ruff==0.14.6 diff --git a/scripts/check-format b/scripts/check-format new file mode 100755 index 0000000..4e8e6a5 --- /dev/null +++ b/scripts/check-format @@ -0,0 +1,7 @@ +#!/usr/bin/env bash +set -euo pipefail + +PYTHON="${PYTHON:-python3}" + +"${PYTHON}" -m ruff format --check . +"${PYTHON}" -m ruff check . diff --git a/scripts/format b/scripts/format new file mode 100755 index 0000000..7cfa30e --- /dev/null +++ b/scripts/format @@ -0,0 +1,7 @@ +#!/usr/bin/env bash +set -euo pipefail + +PYTHON="${PYTHON:-python3}" + +"${PYTHON}" -m ruff format . +"${PYTHON}" -m ruff check --fix . diff --git a/setup.py b/setup.py index 751c66e..dc1dca2 100644 --- a/setup.py +++ b/setup.py @@ -1,46 +1,55 @@ import os from glob import glob + from setuptools import find_packages, setup -package_name = "mavinsight" -runtime_resource_folder = f"share/{package_name}/package_resources" -vehicle_configs = [f for f in glob(os.path.join("vehicles", "**/*"), recursive=True) if os.path.isfile(f)] -sensor_configs = [f for f in glob(os.path.join("sensors", "**/*"), recursive=True) if os.path.isfile(f)] -resource_configs = [f for f in glob(os.path.join("resource", "**/*"), recursive=True) if os.path.isfile(f)] -site_configs = [f for f in glob(os.path.join("sites", "**/**"), recursive=True) if os.path.isfile(f)] +package_name = 'mavinsight' +runtime_resource_folder = f'share/{package_name}/package_resources' +vehicle_configs = [ + f for f in glob(os.path.join('vehicles', '**/*'), recursive=True) if os.path.isfile(f) +] +sensor_configs = [ + f for f in glob(os.path.join('sensors', '**/*'), recursive=True) if os.path.isfile(f) +] +resource_configs = [ + f for f in glob(os.path.join('resource', '**/*'), recursive=True) if os.path.isfile(f) +] +site_configs = [ + f for f in glob(os.path.join('sites', '**/**'), recursive=True) if os.path.isfile(f) +] setup( name=package_name, - version="0.0.0", + version='0.0.0', packages=find_packages( - exclude=["test"], - include=["mavinsight", "mavinsight.*", "models", "models.*"]), + exclude=['test'], include=['mavinsight', 'mavinsight.*', 'models', 'models.*'] + ), data_files=[ - ("share/ament_index/resource_index/packages", ['resource/' + package_name]), - ("share/" + package_name, ["package.xml"]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), (runtime_resource_folder, vehicle_configs), (runtime_resource_folder, sensor_configs), (runtime_resource_folder, resource_configs), (runtime_resource_folder, site_configs), - (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), ], - install_requires=["setuptools"], + install_requires=['setuptools'], zip_safe=True, - maintainer="cdenihan", - maintainer_email="cdenihan@proton.me", - description="TODO: Package description", - license="Apache-2.0", + maintainer='cdenihan', + maintainer_email='cdenihan@proton.me', + description='TODO: Package description', + license='Apache-2.0', entry_points={ - "console_scripts": [ - "graph_member = models.graph_member:GraphMember.main", - "frame_member = models.frame_member:FrameMember.main", - "vehicle = models.vehicle:Vehicle.main", - "sensor = models.sensor:Sensor.main", - "camera = models.sensor:Camera.main", - "gimbal = models.sensor:Gimbal.main", - "rangefinder = models.sensor:Rangefinder.main", - "site = models.site:Site.main", - "tba_viz = models.tba_viz:TBA_Viz.main" + 'console_scripts': [ + 'graph_member = models.graph_member:GraphMember.main', + 'frame_member = models.frame_member:FrameMember.main', + 'vehicle = models.vehicle:Vehicle.main', + 'sensor = models.sensor:Sensor.main', + 'camera = models.sensor:Camera.main', + 'gimbal = models.sensor:Gimbal.main', + 'rangefinder = models.sensor:Rangefinder.main', + 'site = models.site:Site.main', + 'tba_viz = models.tba_viz:TBA_Viz.main', ], }, ) diff --git a/test/camera_test.py b/test/camera_test.py index b687e37..8a8e0b1 100644 --- a/test/camera_test.py +++ b/test/camera_test.py @@ -1,17 +1,19 @@ import os import unittest + from models.sensor import Camera, sensor_factory from models.sensor_types import SensorTypes + class TestCamera(unittest.TestCase): def setUp(self): self.share_dir = os.path.dirname(__file__) - self.sensor_dir = os.path.join(self.share_dir, "sensors") + self.sensor_dir = os.path.join(self.share_dir, 'sensors') - self.cm_dir = os.path.join(self.sensor_dir, "cameras") + self.cm_dir = os.path.join(self.sensor_dir, 'cameras') - self.full_camera: Camera = sensor_factory(os.path.join(self.cm_dir, "full_camera.yaml")) + self.full_camera: Camera = sensor_factory(os.path.join(self.cm_dir, 'full_camera.yaml')) def test_sensor_factory(self): """sensor factory correctly makes camera""" @@ -19,29 +21,39 @@ def test_sensor_factory(self): def test_camera_from_yaml(self): """camera creation from yaml""" - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_no_name.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_no_frame.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_no_parent.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_no_topic.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_no_type.yaml")) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_no_name.yaml')) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_no_frame.yaml')) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_no_parent.yaml') + ) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_no_topic.yaml')) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_no_type.yaml')) # offset cases - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_long_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_short_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_none_in_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.cm_dir, "cm_bad_offset.yaml")) - - cm0 = sensor_factory(os.path.join(self.cm_dir, "cm_no_offset.yaml")) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_long_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_short_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_none_in_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.cm_dir, 'cm_bad_offset.yaml') + ) + + cm0 = sensor_factory(os.path.join(self.cm_dir, 'cm_no_offset.yaml')) self.assertEqual(cm0.offset[0], 0.0) self.assertEqual(cm0.offset[1], 0.0) self.assertEqual(cm0.offset[2], 0.0) - self.assertEqual(self.full_camera.name, "Full test cam") - self.assertEqual(self.full_camera.frame_name, "test_cam") + self.assertEqual(self.full_camera.name, 'Full test cam') + self.assertEqual(self.full_camera.frame_name, 'test_cam') self.assertEqual(len(self.full_camera.offset), 3) self.assertEqual(self.full_camera.offset[0], 9.0) self.assertEqual(self.full_camera.offset[1], 8.0) self.assertEqual(self.full_camera.offset[2], 7.6) - self.assertEqual(self.full_camera.cam_info_topic, "/test/cam") - self.assertEqual(self.full_camera.parent_frame, "link") + self.assertEqual(self.full_camera.cam_info_topic, '/test/cam') + self.assertEqual(self.full_camera.parent_frame, 'link') self.assertEqual(self.full_camera.sensor_type, SensorTypes.CAMERA) diff --git a/test/gimbal_test.py b/test/gimbal_test.py index f394ade..3ad53a1 100644 --- a/test/gimbal_test.py +++ b/test/gimbal_test.py @@ -1,21 +1,25 @@ import os import unittest + from models.sensor import Camera, Gimbal, Rangefinder, sensor_factory from models.sensor_types import SensorTypes + class TestGimbal(unittest.TestCase): def setUp(self): self.share_dir = os.path.dirname(__file__) - self.sensor_dir = os.path.join(self.share_dir, "sensors") + self.sensor_dir = os.path.join(self.share_dir, 'sensors') - self.cm_dir = os.path.join(self.sensor_dir, "cameras") - self.gb_dir = os.path.join(self.sensor_dir, "gimbals") - self.rf_dir = os.path.join(self.sensor_dir, "rangefinders") + self.cm_dir = os.path.join(self.sensor_dir, 'cameras') + self.gb_dir = os.path.join(self.sensor_dir, 'gimbals') + self.rf_dir = os.path.join(self.sensor_dir, 'rangefinders') - self.full_camera: Camera = sensor_factory(os.path.join(self.cm_dir, "full_camera.yaml")) - self.full_gimbal: Gimbal = sensor_factory(os.path.join(self.gb_dir, "full_gimbal.yaml")) - self.full_rangefinder: Rangefinder = sensor_factory(os.path.join(self.rf_dir, "full_rangefinder.yaml")) + self.full_camera: Camera = sensor_factory(os.path.join(self.cm_dir, 'full_camera.yaml')) + self.full_gimbal: Gimbal = sensor_factory(os.path.join(self.gb_dir, 'full_gimbal.yaml')) + self.full_rangefinder: Rangefinder = sensor_factory( + os.path.join(self.rf_dir, 'full_rangefinder.yaml') + ) def test_sensor_factory(self): """sensor factory correctly makes gimbal""" @@ -23,56 +27,66 @@ def test_sensor_factory(self): def test_gimbal_from_yaml(self): """gimbal creation from yaml""" - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_no_name.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_no_frame.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_no_parent.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_no_topic.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_no_type.yaml")) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_no_name.yaml')) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_no_frame.yaml')) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_no_parent.yaml') + ) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_no_topic.yaml')) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_no_type.yaml')) # offset cases - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_long_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_short_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_none_in_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.gb_dir, "gb_bad_offset.yaml")) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_long_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_short_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_none_in_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.gb_dir, 'gb_bad_offset.yaml') + ) - gb0 = sensor_factory(os.path.join(self.gb_dir, "gb_no_offset.yaml")) + gb0 = sensor_factory(os.path.join(self.gb_dir, 'gb_no_offset.yaml')) self.assertEqual(gb0.offset[0], 0.0) self.assertEqual(gb0.offset[1], 0.0) self.assertEqual(gb0.offset[2], 0.0) - gbe = sensor_factory(os.path.join(self.gb_dir, "gb_empty_offset.yaml")) + gbe = sensor_factory(os.path.join(self.gb_dir, 'gb_empty_offset.yaml')) self.assertEqual(gbe.offset[0], 0.0) self.assertEqual(gbe.offset[1], 0.0) self.assertEqual(gbe.offset[2], 0.0) - self.assertEqual(self.full_gimbal.name, "Full Gimbal") - self.assertEqual(self.full_gimbal.frame_name, "full_gimbal") + self.assertEqual(self.full_gimbal.name, 'Full Gimbal') + self.assertEqual(self.full_gimbal.frame_name, 'full_gimbal') self.assertEqual(len(self.full_gimbal.offset), 3) self.assertEqual(self.full_gimbal.offset[0], 0.01) self.assertEqual(self.full_gimbal.offset[1], 0.002) self.assertEqual(self.full_gimbal.offset[2], 0.0003) - self.assertEqual(self.full_gimbal.orientation_topic, "/test/gimbal") - self.assertEqual(self.full_gimbal.parent_frame, "link") + self.assertEqual(self.full_gimbal.orientation_topic, '/test/gimbal') + self.assertEqual(self.full_gimbal.parent_frame, 'link') self.assertEqual(self.full_gimbal.sensor_type, SensorTypes.GIMBAL) sub_cam = self.full_gimbal.sensors[0] - self.assertEqual(sub_cam.name, "Full test cam") - self.assertEqual(sub_cam.frame_name, "test_cam") + self.assertEqual(sub_cam.name, 'Full test cam') + self.assertEqual(sub_cam.frame_name, 'test_cam') self.assertEqual(len(sub_cam.offset), 3) self.assertEqual(sub_cam.offset[0], 9.0) self.assertEqual(sub_cam.offset[1], 8.0) self.assertEqual(sub_cam.offset[2], 7.6) - self.assertEqual(sub_cam.cam_info_topic, "/test/cam") - self.assertEqual(sub_cam.parent_frame, "link") + self.assertEqual(sub_cam.cam_info_topic, '/test/cam') + self.assertEqual(sub_cam.parent_frame, 'link') self.assertEqual(sub_cam.sensor_type, SensorTypes.CAMERA) sub_rf = self.full_gimbal.sensors[1] - self.assertEqual(sub_rf.name, "test full rangefinder") - self.assertEqual(sub_rf.frame_name, "test_rangefinder") + self.assertEqual(sub_rf.name, 'test full rangefinder') + self.assertEqual(sub_rf.frame_name, 'test_rangefinder') self.assertEqual(len(sub_rf.offset), 3) self.assertEqual(sub_rf.offset[0], 1.0) self.assertEqual(sub_rf.offset[1], 2.0) self.assertEqual(sub_rf.offset[2], 3.0) - self.assertEqual(sub_rf.range_topic, "/test/rangefinder") - self.assertEqual(sub_rf.parent_frame, "link") + self.assertEqual(sub_rf.range_topic, '/test/rangefinder') + self.assertEqual(sub_rf.parent_frame, 'link') self.assertEqual(sub_rf.sensor_type, SensorTypes.RANGEFINDER) diff --git a/test/rangefinder_test.py b/test/rangefinder_test.py index 4a81ab7..5305e2f 100644 --- a/test/rangefinder_test.py +++ b/test/rangefinder_test.py @@ -1,17 +1,21 @@ import os import unittest + from models.sensor import Rangefinder, sensor_factory from models.sensor_types import SensorTypes + class TestRangefinder(unittest.TestCase): def setUp(self): self.share_dir = os.path.dirname(__file__) - self.sensor_dir = os.path.join(self.share_dir, "sensors") + self.sensor_dir = os.path.join(self.share_dir, 'sensors') - self.rf_dir = os.path.join(self.sensor_dir, "rangefinders") + self.rf_dir = os.path.join(self.sensor_dir, 'rangefinders') - self.full_rangefinder: Rangefinder = sensor_factory(os.path.join(self.rf_dir, "full_rangefinder.yaml")) + self.full_rangefinder: Rangefinder = sensor_factory( + os.path.join(self.rf_dir, 'full_rangefinder.yaml') + ) def test_sensor_factory(self): """sensor factory correctly makes rangefinder""" @@ -19,29 +23,39 @@ def test_sensor_factory(self): def test_rangefinder_from_yaml(self): """rangefinder creation from yaml""" - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_no_name.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_no_frame.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_no_parent.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_no_topic.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_no_type.yaml")) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_no_name.yaml')) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_no_frame.yaml')) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_no_parent.yaml') + ) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_no_topic.yaml')) + self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_no_type.yaml')) # offset cases - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_long_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_short_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_none_in_offset.yaml")) - self.assertRaises(ValueError, sensor_factory, os.path.join(self.rf_dir, "rf_bad_offset.yaml")) - - rf0 = sensor_factory(os.path.join(self.rf_dir, "rf_no_offset.yaml")) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_long_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_short_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_none_in_offset.yaml') + ) + self.assertRaises( + ValueError, sensor_factory, os.path.join(self.rf_dir, 'rf_bad_offset.yaml') + ) + + rf0 = sensor_factory(os.path.join(self.rf_dir, 'rf_no_offset.yaml')) self.assertEqual(rf0.offset[0], 0.0) self.assertEqual(rf0.offset[1], 0.0) self.assertEqual(rf0.offset[2], 0.0) - self.assertEqual(self.full_rangefinder.name, "test full rangefinder") - self.assertEqual(self.full_rangefinder.frame_name, "test_rangefinder") + self.assertEqual(self.full_rangefinder.name, 'test full rangefinder') + self.assertEqual(self.full_rangefinder.frame_name, 'test_rangefinder') self.assertEqual(len(self.full_rangefinder.offset), 3) self.assertEqual(self.full_rangefinder.offset[0], 1.0) self.assertEqual(self.full_rangefinder.offset[1], 2.0) self.assertEqual(self.full_rangefinder.offset[2], 3.0) - self.assertEqual(self.full_rangefinder.range_topic, "/test/rangefinder") - self.assertEqual(self.full_rangefinder.parent_frame, "link") + self.assertEqual(self.full_rangefinder.range_topic, '/test/rangefinder') + self.assertEqual(self.full_rangefinder.parent_frame, 'link') self.assertEqual(self.full_rangefinder.sensor_type, SensorTypes.RANGEFINDER) diff --git a/test/test_copyright.py b/test/test_copyright.py index 95f0381..bdf595e 100644 --- a/test/test_copyright.py +++ b/test/test_copyright.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_copyright.main import main import pytest +from ament_copyright.main import main # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py index 49c1644..83ad974 100644 --- a/test/test_flake8.py +++ b/test/test_flake8.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_flake8.main import main_with_errors import pytest +from ament_flake8.main import main_with_errors @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len(errors) + '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py index a2c3deb..6b6d214 100644 --- a/test/test_pep257.py +++ b/test/test_pep257.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_pep257.main import main import pytest +from ament_pep257.main import main @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/test/vehicle_test.py b/test/vehicle_test.py index d9369f6..c16d32e 100644 --- a/test/vehicle_test.py +++ b/test/vehicle_test.py @@ -1,68 +1,82 @@ import os import unittest + from models.platforms import Platforms from models.sensor import Camera, Gimbal, Rangefinder, SensorTypes from models.vehicle import Vehicle, vehicle_factory + class TestVehicle(unittest.TestCase): def setUp(self): self.share_dir = os.path.dirname(__file__) - self.vehicle_dir = os.path.join(self.share_dir, "vehicles") - self.bad_v_dir = os.path.join(self.vehicle_dir, "bad_vehicles") + self.vehicle_dir = os.path.join(self.share_dir, 'vehicles') + self.bad_v_dir = os.path.join(self.vehicle_dir, 'bad_vehicles') - self.full_vehicle: Vehicle = vehicle_factory(os.path.join(self.vehicle_dir, "full_vehicle.yaml")) - self.minimum_vehicle: Vehicle = vehicle_factory(os.path.join(self.vehicle_dir, "minimum_vehicle.yaml")) + self.full_vehicle: Vehicle = vehicle_factory( + os.path.join(self.vehicle_dir, 'full_vehicle.yaml') + ) + self.minimum_vehicle: Vehicle = vehicle_factory( + os.path.join(self.vehicle_dir, 'minimum_vehicle.yaml') + ) def test_vehicle_factory(self): """vehicle factory correctly makes or doesnt make gimbal""" self.assertIsInstance(self.full_vehicle, Vehicle) - self.assertRaises(FileNotFoundError, vehicle_factory, "not_a_file") + self.assertRaises(FileNotFoundError, vehicle_factory, 'not_a_file') def test_vehicle_from_yaml(self): """vehicle creation from yaml""" - self.assertRaises(ValueError, vehicle_factory, os.path.join(self.bad_v_dir, "v_no_name.yaml")) - self.assertRaises(ValueError, vehicle_factory, os.path.join(self.bad_v_dir, "v_no_frame.yaml")) - self.assertRaises(ValueError, vehicle_factory, os.path.join(self.bad_v_dir, "v_no_topic.yaml")) - self.assertRaises(ValueError, vehicle_factory, os.path.join(self.bad_v_dir, "v_no_platform.yaml")) + self.assertRaises( + ValueError, vehicle_factory, os.path.join(self.bad_v_dir, 'v_no_name.yaml') + ) + self.assertRaises( + ValueError, vehicle_factory, os.path.join(self.bad_v_dir, 'v_no_frame.yaml') + ) + self.assertRaises( + ValueError, vehicle_factory, os.path.join(self.bad_v_dir, 'v_no_topic.yaml') + ) + self.assertRaises( + ValueError, vehicle_factory, os.path.join(self.bad_v_dir, 'v_no_platform.yaml') + ) # no parent case (guarded) - vp = vehicle_factory(os.path.join(self.bad_v_dir, "v_no_parent.yaml")) - self.assertEqual(vp.parent_frame, "map") + vp = vehicle_factory(os.path.join(self.bad_v_dir, 'v_no_parent.yaml')) + self.assertEqual(vp.parent_frame, 'map') # no sensors case (guarded) - vns = vehicle_factory(os.path.join(self.bad_v_dir, "v_no_sensors.yaml")) + vns = vehicle_factory(os.path.join(self.bad_v_dir, 'v_no_sensors.yaml')) self.assertIsInstance(vns.sensors, list) self.assertEqual(len(vns.sensors), 0) # empty sensors case (guarded) - ves = vehicle_factory(os.path.join(self.bad_v_dir, "v_empty_sensors.yaml")) + ves = vehicle_factory(os.path.join(self.bad_v_dir, 'v_empty_sensors.yaml')) self.assertIsInstance(ves.sensors, list) self.assertEqual(len(ves.sensors), 0) # minimum vehicle case (all guarded params removed) - vm = vehicle_factory(os.path.join(self.vehicle_dir, "minimum_vehicle.yaml")) - self.assertEqual(vm.name, "Minimum Vehicle") - self.assertEqual(vm.frame_name, "minimum_vehicle_base_link") - self.assertEqual(vm.location_topic, "/minimum/vehicle/fix") - self.assertEqual(vm.parent_frame, "map") + vm = vehicle_factory(os.path.join(self.vehicle_dir, 'minimum_vehicle.yaml')) + self.assertEqual(vm.name, 'Minimum Vehicle') + self.assertEqual(vm.frame_name, 'minimum_vehicle_base_link') + self.assertEqual(vm.location_topic, '/minimum/vehicle/fix') + self.assertEqual(vm.parent_frame, 'map') self.assertEqual(vm.platform, Platforms.QUAD_COPTER) self.assertIsInstance(vm.sensors, list) self.assertEqual(len(vm.sensors), 0) # full vehicle checking - self.assertEqual(self.full_vehicle.name, "Full Vehicle") - self.assertEqual(self.full_vehicle.frame_name, "full_vehicle_base_link") - self.assertEqual(self.full_vehicle.location_topic, "/full/vehicle/fix") - self.assertEqual(self.full_vehicle.parent_frame, "full_vehicle_parent") + self.assertEqual(self.full_vehicle.name, 'Full Vehicle') + self.assertEqual(self.full_vehicle.frame_name, 'full_vehicle_base_link') + self.assertEqual(self.full_vehicle.location_topic, '/full/vehicle/fix') + self.assertEqual(self.full_vehicle.parent_frame, 'full_vehicle_parent') self.assertEqual(self.full_vehicle.platform, Platforms.QUAD_COPTER) self.assertEqual(len(self.full_vehicle.sensors), 2) sub_cam: Camera = self.full_vehicle.sensors[0] - self.assertEqual(sub_cam.name, "No Offset Camera") - self.assertEqual(sub_cam.frame_name, "no_offset_cam") - self.assertEqual(sub_cam.cam_info_topic, "/no/offset/cam") - self.assertEqual(sub_cam.parent_frame, "link") + self.assertEqual(sub_cam.name, 'No Offset Camera') + self.assertEqual(sub_cam.frame_name, 'no_offset_cam') + self.assertEqual(sub_cam.cam_info_topic, '/no/offset/cam') + self.assertEqual(sub_cam.parent_frame, 'link') self.assertEqual(sub_cam.sensor_type, SensorTypes.CAMERA) self.assertEqual(len(sub_cam.offset), 3) self.assertEqual(sub_cam.offset[0], 0.0) @@ -70,10 +84,10 @@ def test_vehicle_from_yaml(self): self.assertEqual(sub_cam.offset[2], 0.0) sub_gimbal: Gimbal = self.full_vehicle.sensors[1] - self.assertEqual(sub_gimbal.name, "Full Gimbal") - self.assertEqual(sub_gimbal.frame_name, "full_gimbal") - self.assertEqual(sub_gimbal.orientation_topic, "/test/gimbal") - self.assertEqual(sub_gimbal.parent_frame, "link") + self.assertEqual(sub_gimbal.name, 'Full Gimbal') + self.assertEqual(sub_gimbal.frame_name, 'full_gimbal') + self.assertEqual(sub_gimbal.orientation_topic, '/test/gimbal') + self.assertEqual(sub_gimbal.parent_frame, 'link') self.assertEqual(sub_gimbal.sensor_type, SensorTypes.GIMBAL) self.assertEqual(len(sub_gimbal.offset), 3) self.assertEqual(sub_gimbal.offset[0], 0.01) @@ -81,23 +95,23 @@ def test_vehicle_from_yaml(self): self.assertEqual(sub_gimbal.offset[2], 0.0003) g_cam: Camera = self.full_vehicle.sensors[1].sensors[0] - self.assertEqual(g_cam.name, "Full test cam") - self.assertEqual(g_cam.frame_name, "test_cam") + self.assertEqual(g_cam.name, 'Full test cam') + self.assertEqual(g_cam.frame_name, 'test_cam') self.assertEqual(len(g_cam.offset), 3) self.assertEqual(g_cam.offset[0], 9.0) self.assertEqual(g_cam.offset[1], 8.0) self.assertEqual(g_cam.offset[2], 7.6) - self.assertEqual(g_cam.cam_info_topic, "/test/cam") - self.assertEqual(g_cam.parent_frame, "link") + self.assertEqual(g_cam.cam_info_topic, '/test/cam') + self.assertEqual(g_cam.parent_frame, 'link') self.assertEqual(g_cam.sensor_type, SensorTypes.CAMERA) g_rf: Rangefinder = self.full_vehicle.sensors[1].sensors[1] - self.assertEqual(g_rf.name, "test full rangefinder") - self.assertEqual(g_rf.frame_name, "test_rangefinder") + self.assertEqual(g_rf.name, 'test full rangefinder') + self.assertEqual(g_rf.frame_name, 'test_rangefinder') self.assertEqual(len(g_rf.offset), 3) self.assertEqual(g_rf.offset[0], 1.0) self.assertEqual(g_rf.offset[1], 2.0) self.assertEqual(g_rf.offset[2], 3.0) - self.assertEqual(g_rf.range_topic, "/test/rangefinder") - self.assertEqual(g_rf.parent_frame, "link") + self.assertEqual(g_rf.range_topic, '/test/rangefinder') + self.assertEqual(g_rf.parent_frame, 'link') self.assertEqual(g_rf.sensor_type, SensorTypes.RANGEFINDER)