From 8bd6735a1a8ac2115983b766fad3eb89c75a8339 Mon Sep 17 00:00:00 2001 From: aestene Date: Sun, 4 Jun 2023 13:32:56 +0200 Subject: [PATCH] Adapt repository to new exception handling --- src/isar_turtlebot/ros_bridge/ros_bridge.py | 16 ++++--- src/isar_turtlebot/ros_bridge/topic.py | 10 ++++- src/isar_turtlebot/services/video_streamer.py | 4 +- src/isar_turtlebot/settings/settings.py | 2 + .../turtlebot/step_handlers/driveto.py | 14 +++++- .../turtlebot/step_handlers/stephandler.py | 3 ++ .../turtlebot/step_handlers/takeimage.py | 20 +++++++-- .../step_handlers/takethermalimage.py | 22 +++++++--- src/isar_turtlebot/turtlebot/turtlebot.py | 43 ++++++++++--------- 9 files changed, 94 insertions(+), 40 deletions(-) diff --git a/src/isar_turtlebot/ros_bridge/ros_bridge.py b/src/isar_turtlebot/ros_bridge/ros_bridge.py index fbd7dba..9bf2f4a 100644 --- a/src/isar_turtlebot/ros_bridge/ros_bridge.py +++ b/src/isar_turtlebot/ros_bridge/ros_bridge.py @@ -2,6 +2,9 @@ from abc import ABC from logging import Logger +from robot_interface.models.exceptions.robot_exceptions import ( + RobotCommunicationException, +) from roslibpy import Ros from isar_turtlebot.ros_bridge.topic import ImageTopic, Topic @@ -74,18 +77,19 @@ def connect_client( self.logger.info(f"Successfully connected to ROS at {host}:{port}.") break except Exception as e: - msg: str = ( - "RosBridge failed to connect to ROS at" - f" {host}:{port} with message: {e}" + error_description: str = ( + "RosBridge failed to connect to ROS at {host}:{port} with message" ) self.logger.warning( - f"{msg} - Attempt {connection_retries - retries}" + f"{error_description} - Attempt {connection_retries - retries}" f" of {connection_retries}" ) if not retries: - self.logger.error(msg) - raise ConnectionError(msg) + self.logger.exception(error_description) + raise RobotCommunicationException( + error_description=error_description + ) client.run() diff --git a/src/isar_turtlebot/ros_bridge/topic.py b/src/isar_turtlebot/ros_bridge/topic.py index 80d67cd..27c243c 100644 --- a/src/isar_turtlebot/ros_bridge/topic.py +++ b/src/isar_turtlebot/ros_bridge/topic.py @@ -4,6 +4,9 @@ from logging import Logger from typing import Any, Optional +from robot_interface.models.exceptions.robot_exceptions import ( + RobotRetrieveInspectionException, +) from roslibpy import Message, Ros from roslibpy import Topic as RosTopic @@ -123,6 +126,11 @@ def get_image(self) -> Optional[bytes]: time.sleep(0.1) execution_time: float = time.time() - start_time if execution_time > self.get_image_timeout: - raise TimeoutError("Unable to read image data from topic") + error_description: str = "Timed our while fetching the image" + self.logger.error(error_description) + raise RobotRetrieveInspectionException( + error_description=error_description + ) + self.take_image = False return self.image diff --git a/src/isar_turtlebot/services/video_streamer.py b/src/isar_turtlebot/services/video_streamer.py index 5f6ac04..403e4c6 100644 --- a/src/isar_turtlebot/services/video_streamer.py +++ b/src/isar_turtlebot/services/video_streamer.py @@ -7,10 +7,8 @@ class VideoStreamer: def __init__(self, bridge: RosBridge) -> None: self.bridge: RosBridge = bridge - def main(self): + def main(self) -> None: while True: image_data: str = self.bridge.video_stream.get_image() image_bytes: bytes = base64.b64decode(image_data) print("Streaming video") - - return diff --git a/src/isar_turtlebot/settings/settings.py b/src/isar_turtlebot/settings/settings.py index 3526c15..2e79073 100644 --- a/src/isar_turtlebot/settings/settings.py +++ b/src/isar_turtlebot/settings/settings.py @@ -15,6 +15,8 @@ class Settings(BaseSettings): INSPECTION_POSE_TIMEOUT: int = Field(default=60) GET_IMAGE_TIMEOUT: int = Field(default=5) + LOGGER_NAME: str = Field(default="isar_turtlebot") + class Config: env_prefix = "ISAR_TURTLEBOT_" diff --git a/src/isar_turtlebot/turtlebot/step_handlers/driveto.py b/src/isar_turtlebot/turtlebot/step_handlers/driveto.py index 4902281..8041378 100644 --- a/src/isar_turtlebot/turtlebot/step_handlers/driveto.py +++ b/src/isar_turtlebot/turtlebot/step_handlers/driveto.py @@ -1,7 +1,12 @@ +import logging import time +from logging import Logger from typing import Optional from alitra import Frame, Pose, Transform +from robot_interface.models.exceptions.robot_exceptions import ( + RobotCommunicationException, +) from robot_interface.models.mission.step import DriveToPose from isar_turtlebot.models.turtlebot_status import Status @@ -18,10 +23,14 @@ def __init__( transform: Transform, publishing_timeout: float = settings.PUBLISHING_TIMEOUT, ) -> None: + self.logger: Logger = logging.getLogger(settings.LOGGER_NAME) + self.bridge: RosBridge = bridge self.transform: Transform = transform self.publishing_timeout: float = publishing_timeout + self.goal_id: Optional[str] = None + def start( self, step: DriveToPose, @@ -38,7 +47,10 @@ def start( while self._goal_id() == goal_id: time.sleep(0.1) if (time.time() - start_time) > self.publishing_timeout: - raise TimeoutError("Publishing navigation message timed out") + error_description: str = "Publishing navigation message timed out" + self.logger.error(error_description) + raise RobotCommunicationException(error_description=error_description) + self.goal_id = self._goal_id() def _goal_id(self) -> Optional[str]: diff --git a/src/isar_turtlebot/turtlebot/step_handlers/stephandler.py b/src/isar_turtlebot/turtlebot/step_handlers/stephandler.py index 8f4455c..a10d45e 100644 --- a/src/isar_turtlebot/turtlebot/step_handlers/stephandler.py +++ b/src/isar_turtlebot/turtlebot/step_handlers/stephandler.py @@ -1,9 +1,12 @@ +import logging from abc import ABC, abstractmethod from typing import Dict +from robot_interface.models.exceptions.robot_exceptions import RobotAPIException from robot_interface.models.mission.step import Step from isar_turtlebot.models.turtlebot_status import Status +from isar_turtlebot.settings import settings class StepHandler(ABC): diff --git a/src/isar_turtlebot/turtlebot/step_handlers/takeimage.py b/src/isar_turtlebot/turtlebot/step_handlers/takeimage.py index 07cbbde..ab3776d 100644 --- a/src/isar_turtlebot/turtlebot/step_handlers/takeimage.py +++ b/src/isar_turtlebot/turtlebot/step_handlers/takeimage.py @@ -1,11 +1,17 @@ import base64 +import logging import time from datetime import datetime +from logging import Logger from pathlib import Path from typing import Optional from uuid import uuid4 from alitra import Pose, Position, Transform +from robot_interface.models.exceptions.robot_exceptions import ( + RobotCommunicationException, + RobotInfeasibleStepException, +) from robot_interface.models.inspection.inspection import ( Image, ImageMetadata, @@ -33,6 +39,8 @@ def __init__( publishing_timeout: float = settings.PUBLISHING_TIMEOUT, inspection_pose_timeout: float = settings.INSPECTION_POSE_TIMEOUT, ) -> None: + self.logger: Logger = logging.getLogger(settings.LOGGER_NAME) + self.bridge: RosBridge = bridge self.transform: Transform = transform self.storage_folder: Path = storage_folder @@ -67,7 +75,9 @@ def start(self, step: TakeImage) -> None: time.sleep(0.1) if (time.time() - start_time) > self.publishing_timeout: self.status = Status.Failure - raise TimeoutError("Publishing navigation message timed out.") + error_description: str = "Publishing navigation message timed out" + self.logger.error(error_description) + raise RobotCommunicationException(error_description=error_description) start_time = time.time() while self._move_status() is not Status.Succeeded: @@ -75,7 +85,9 @@ def start(self, step: TakeImage) -> None: execution_time: float = time.time() - start_time if execution_time > self.inspection_pose_timeout: self.status = Status.Failure - raise TimeoutError("Navigation to inspection pose timed out.") + error_description = "Navigation to inspection pose timed out" + self.logger.error(error_description) + raise RobotInfeasibleStepException(error_description=error_description) self._write_image_bytes() @@ -124,10 +136,10 @@ def _move_status(self) -> Status: ) return move_status - def _write_image_bytes(self): + def _write_image_bytes(self) -> None: image_data: str = self.bridge.visual_inspection.get_image() image_bytes: bytes = base64.b64decode(image_data) - self.filename: Path = Path( + self.filename = Path( f"{self.storage_folder.as_posix()}/{str(uuid4())}.{self.image_filetype}" ) diff --git a/src/isar_turtlebot/turtlebot/step_handlers/takethermalimage.py b/src/isar_turtlebot/turtlebot/step_handlers/takethermalimage.py index 10207b6..581d129 100644 --- a/src/isar_turtlebot/turtlebot/step_handlers/takethermalimage.py +++ b/src/isar_turtlebot/turtlebot/step_handlers/takethermalimage.py @@ -1,7 +1,9 @@ import base64 +import logging import time from datetime import datetime from io import BytesIO +from logging import Logger from pathlib import Path from typing import Optional from uuid import uuid4 @@ -9,6 +11,10 @@ import PIL.Image as PILImage import numpy as np from alitra import Pose, Position, Transform +from robot_interface.models.exceptions.robot_exceptions import ( + RobotCommunicationException, + RobotInfeasibleStepException, +) from robot_interface.models.inspection.inspection import ( ThermalImage, ThermalImageMetadata, @@ -36,6 +42,8 @@ def __init__( publishing_timeout: float = settings.PUBLISHING_TIMEOUT, inspection_pose_timeout: float = settings.INSPECTION_POSE_TIMEOUT, ) -> None: + self.logger: Logger = logging.getLogger(settings.LOGGER_NAME) + self.bridge: RosBridge = bridge self.transform: Transform = transform self.storage_folder: Path = storage_folder @@ -72,7 +80,9 @@ def start( time.sleep(0.1) if (time.time() - start_time) > self.publishing_timeout: self.status = Status.Failure - raise TimeoutError("Publishing navigation message timed out.") + error_description: str = "Publishing navigation message timed out" + self.logger.error(error_description) + raise RobotCommunicationException(error_description=error_description) start_time = time.time() while self._move_status() is not Status.Succeeded: @@ -80,7 +90,9 @@ def start( execution_time: float = time.time() - start_time if execution_time > self.inspection_pose_timeout: self.status = Status.Failure - raise TimeoutError("Navigation to inspection pose timed out.") + error_description = "Navigation to inspection pose timed out" + self.logger.error(error_description) + raise RobotInfeasibleStepException(error_description=error_description) self._write_image_bytes() @@ -129,12 +141,12 @@ def _move_status(self) -> Status: ) return move_status - def _write_image_bytes(self): + def _write_image_bytes(self) -> None: encoded_image_data: bytes = self.bridge.visual_inspection.get_image() image_bytes: bytes = base64.b64decode(encoded_image_data) - image_bytes: bytes = self._convert_to_thermal(image_bytes) + image_bytes = self._convert_to_thermal(image_bytes) - self.filename: Path = Path( + self.filename = Path( f"{self.storage_folder.as_posix()}/{str(uuid4())}" f".{self.thermal_image_filetype}" ) diff --git a/src/isar_turtlebot/turtlebot/turtlebot.py b/src/isar_turtlebot/turtlebot/turtlebot.py index 36e4dcb..0a4e626 100644 --- a/src/isar_turtlebot/turtlebot/turtlebot.py +++ b/src/isar_turtlebot/turtlebot/turtlebot.py @@ -7,13 +7,11 @@ from uuid import UUID from alitra import Frame, Pose, Transform -from robot_interface.models.exceptions import ( - RobotCommunicationException, - RobotException, -) from robot_interface.models.exceptions.robot_exceptions import ( - RobotInvalidTelemetryException, + RobotRetrieveInspectionException, + RobotTelemetryException, ) + from robot_interface.models.inspection.inspection import Inspection from robot_interface.models.mission.status import StepStatus from robot_interface.models.mission.step import InspectionStep, Step @@ -25,6 +23,7 @@ from isar_turtlebot.models.turtlebot_status import Status from isar_turtlebot.ros_bridge import RosBridge +from isar_turtlebot.settings import settings from isar_turtlebot.turtlebot.step_handlers import ( DriveToHandler, TakeImageHandler, @@ -41,7 +40,7 @@ class Turtlebot: """Step manager for Turtlebot.""" def __init__(self, bridge: RosBridge, transform: Transform) -> None: - self.logger: Logger = logging.getLogger("robot") + self.logger: Logger = logging.getLogger(settings.LOGGER_NAME) self.bridge: RosBridge = bridge self.transform: Transform = transform self.status: Optional[Status] = None @@ -64,10 +63,7 @@ def cancel_step(self) -> None: def publish_step(self, step: Step) -> None: self.step_handler = self.step_handlers[type(step).__name__] - try: - self.step_handler.start(step) - except TimeoutError as e: - raise RobotCommunicationException from e + self.step_handler.start(step) if isinstance(step, InspectionStep): self.filenames[step.id] = self.step_handler.get_filename() @@ -78,17 +74,23 @@ def get_step_status(self) -> StepStatus: status: Status = self.step_handler.get_status() return Status.map_to_step_status(status=status) - def get_inspections(self, id: UUID) -> Sequence[Inspection]: + def get_inspections(self, step_id: UUID) -> Sequence[Inspection]: try: - inspection: Inspection = self.inspections[id] - except KeyError as e: - self.logger.warning(f"No inspection connected to step: {id}!") - raise RobotException from e + inspection: Inspection = self.inspections[step_id] + except KeyError: + error_description: str = f"Failed to collect inspection for step {step_id}" + self.logger.exception(error_description) + raise RobotRetrieveInspectionException(error_description=error_description) + try: - inspection.data = self._read_data(id) - except FileNotFoundError as e: - self.logger.warning(f"No data file connected to step: {id}!") - raise RobotException from e + inspection.data = self._read_data(step_id) + except FileNotFoundError: + error_description = ( + f"Failed to read inspection for step {step_id} from file" + ) + self.logger.exception(error_description) + raise RobotRetrieveInspectionException(error_description=error_description) + return [inspection] def _read_data(self, inspection_id: UUID) -> bytes: @@ -103,7 +105,8 @@ def set_initial_pose(self, pose: Pose) -> None: def get_pose_telemetry(self, robot_name: str, isar_id: str) -> str: pose_turtlebot: dict = self.bridge.pose.get_value() if not pose_turtlebot: - raise RobotInvalidTelemetryException + error_description: str = "Retrieving pose returned a null value" + raise RobotTelemetryException(error_description=error_description) robot_pose: Pose = decode_pose_message(pose_message=pose_turtlebot) pose: Pose = self.transform.transform_pose(