From b0b8a8e40a9a5d38de3b8adfeb818929adcc754e Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 12 Jan 2024 15:47:32 +0100 Subject: [PATCH 1/2] update rae default app --- rae-default/README.md | 6 +- rae-default/robotapp.toml | 8 +- rae-default/src/api/__init__.py | 0 rae-default/src/api/ros/__init__.py | 0 rae-default/src/api/ros/ros2_manager.py | 198 ------------------------ rae-default/src/app.py | 17 +- rae-default/src/robot/__init__.py | 0 rae-default/src/robot/movement.py | 14 -- rae-default/src/robot/robot.py | 81 ---------- rae-default/src/utils/__init__.py | 0 rae-default/src/utils/logging.py | 14 -- rae-default/start_ros.sh | 9 -- 12 files changed, 14 insertions(+), 333 deletions(-) delete mode 100644 rae-default/src/api/__init__.py delete mode 100644 rae-default/src/api/ros/__init__.py delete mode 100644 rae-default/src/api/ros/ros2_manager.py delete mode 100644 rae-default/src/robot/__init__.py delete mode 100644 rae-default/src/robot/movement.py delete mode 100644 rae-default/src/robot/robot.py delete mode 100644 rae-default/src/utils/__init__.py delete mode 100644 rae-default/src/utils/logging.py delete mode 100644 rae-default/start_ros.sh diff --git a/rae-default/README.md b/rae-default/README.md index 98bd1d9..1bb817f 100644 --- a/rae-default/README.md +++ b/rae-default/README.md @@ -1,8 +1,8 @@ # Video Streams -This App uploads a DepthAI pipeline to RAE and streams each camera output to RobotHub. +This App launches robot class and uploads a DepthAI pipeline to RAE to stream each camera output to RobotHub. -You can easily replace with you desired sockets that what to be streamed in `streams` dictionary. Also for configuring the pipeline you can edit `build_pipeline()`, such as resolution, fps and more. +You can easily replace with you desired sockets that what to be streamed in `streams` dictionary. Also for configuring the pipeline you can edit `build_pipeline()`, such as resolution, fps and more. On how to work with Robot class, please refer to [RAE SDK Docs](https://docs-beta.luxonis.com/develop/use-with-ros/rae-sdk/). ## Requirements @@ -10,4 +10,4 @@ You can easily replace with you desired sockets that what to be streamed in `str ## Usage -- Assign devices to the App and launch it, one stream will be started. +- Assign devices to the App and launch it. diff --git a/rae-default/robotapp.toml b/rae-default/robotapp.toml index 922ffad..b48f235 100644 --- a/rae-default/robotapp.toml +++ b/rae-default/robotapp.toml @@ -2,13 +2,13 @@ config_version = "2.0" configuration = [] [info] -name = "RAE Streaming Application" -description = "Streams all cameras" +name = "RAE Default App" +description = "Default App for RAE" [runtime] application = "src/app.py#Application" workdir = "/app" -pre_launch = "export ROS_DOMAIN_ID=30\n. /opt/ros/$ROS_DISTRO/setup.sh\n. /ws/install/setup.sh && export LD_LIBRARY_PATH=/usr/local/lib/python3.10/dist-packages${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +pre_launch = "export ROS_DOMAIN_ID=30\n. /opt/ros/$ROS_DISTRO/setup.sh\n. /ws/install/setup.sh" permissions = ["rae-peripherals"] [runtime.frontend] @@ -16,4 +16,4 @@ redirectToIndex = true [runtime.runs_on] type = "image" -name = "luxonis/rae-ros-robot:dai_ros_py_base" +name = "pull ghcr.io/luxonis/rae-ros:v0.2.0-humble" diff --git a/rae-default/src/api/__init__.py b/rae-default/src/api/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rae-default/src/api/ros/__init__.py b/rae-default/src/api/ros/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rae-default/src/api/ros/ros2_manager.py b/rae-default/src/api/ros/ros2_manager.py deleted file mode 100644 index ca1a965..0000000 --- a/rae-default/src/api/ros/ros2_manager.py +++ /dev/null @@ -1,198 +0,0 @@ -import logging as log -import os -import threading -import rclpy -import time -import math -import subprocess -from time import sleep -import signal - -from typing import Any, Callable, Dict, Type -from rclpy.executors import Executor, MultiThreadedExecutor, SingleThreadedExecutor -from rclpy.publisher import Publisher -from rclpy.subscription import Subscription -from rclpy.client import Client -from rclpy.timer import Timer - - -QOS_PROFILE = 10 -ROS2_NAMESPACE = '/rae' - - -class ROS2Manager: - """ - A class that manages ROS2 functionalities for a robot or a system. It includes initializing ROS2 context, - creating and managing nodes, publishers, and subscribers. It also handles the startup and shutdown processes - for ROS2. - - Attributes: - ros_proc: Process for running ROS2 hardware-related commands. - _name (str): Name of the ROS2 node. - _context (rclpy.context.Context | None): The ROS2 context. - _node (rclpy.node.Node | None): The ROS2 node. - _publishers (dict[str, Publisher]): Dictionary of ROS2 publishers. - _subscribers (dict[str, Subscription]): Dictionary of ROS2 subscribers. - - Methods: - get_node(): Returns the current ROS2 node. - start_hardware_process(): Starts the hardware process for ROS2. - start(): Initializes and starts the ROS2 node and executor. - stop_ros_process(): Stops the ROS2 hardware process. - stop(): Shuts down the ROS2 node and context. - create_publisher(topic_name, msg_type, qos_profile): Creates a publisher for a given topic. - publish(topic_name, msg): Publishes a message on a given topic. - create_subscriber(topic_name, msg_type, callback, qos_profile): Creates a subscriber for a given topic. - """ - def __init__(self, name: str) -> None: - """ - Initializes the ROS2Manager instance. - - Args: - name (str): The name of the ROS2 node. - """ - - self.ros_proc = None - self._name = name - self._context: rclpy.context.Context | None = None - self._node: rclpy.node.Node | None = None - self._publishers: dict[str, Publisher] = {} - self._subscribers: dict[str, Subscription] = {} - self._service_clients: dict[str, Client] = {} - self._timers: dict[str, Timer] = {} - - def get_node(self): - return self._node - - def start_hardware_process(self): - """ - Starts RAE hardware drivers in a separate process. - """ - env = dict(os.environ) - self.ros_proc = subprocess.Popen( - "bash -c 'chmod +x /app/start_ros.sh ; /app/start_ros.sh'", shell=True, env=env, preexec_fn=os.setsid - ) - - def start(self) -> None: - """ - Runs RAE hardware drivers process.Initializes and starts the ROS2 node and executor. It sets up the ROS2 context and starts the ROS2 spin. - """ - self.start_hardware_process() - sleep(2) - self._context = rclpy.Context() - self._context.init() - log.info("ROS2 context initialized.") - - self._node = rclpy.create_node(self._name, context=self._context, namespace=ROS2_NAMESPACE) - log.info(f"Created ROS2 node with name: {self._name}...") - self._executor = SingleThreadedExecutor(context=self._context) - self._executor.add_node(self._node) - self._executor_thread = threading.Thread(target=self._spin) - self._executor_thread.start() - log.info(f"Node started!") - - def _spin(self): - log.info("rlcpy thread> Start") - try: - self._executor.spin() - except Exception as e: - log.info("rlcpy thread> failed") - log.error(e) - log.info("rlcpy thread> Done") - - def stop_ros_process(self): - """ - Stops the ROS2 hardware process by terminating the related subprocess. - """ - - if self.ros_proc is not None: - pgid = os.getpgid(self.ros_proc.pid) - os.killpg(pgid, signal.SIGTERM) - - def stop(self) -> None: - """ - Shuts down RAE drivers, ROS2 node and context. This includes stopping the executor, destroying publishers and subscribers, - and shutting down the ROS2 context. - """ - self.stop_ros_process() - if not self._context: - log.info("ROS2 context is already stopped") - return - - if self._executor_thread: - self._executor.shutdown() - self._executor_thread.join() - self._executor_thread = None - - if self._node: - log.info("Destroying ROS2 node...") - self._destroy_publishers_and_subscribers() - self._node.destroy_node() - self._node = None - - if self._context: - log.info("Shutting down ROS2 context...") - self._context.try_shutdown() - self._context.destroy() - self._context = None - - def _destroy_publishers_and_subscribers(self): - for topic_name, publisher in self._publishers.items(): - log.info(f"Destroying {topic_name} publisher...") - self._node.destroy_publisher(publisher) - - for topic_name, subscriber in self._subscribers.items(): - log.info(f"Destroying {topic_name} subscriber...") - self._node.destroy_subscription(subscriber) - - self._publishers.clear() - self._subscribers.clear() - - def create_publisher(self, topic_name: str, msg_type:Any, qos_profile: int = QOS_PROFILE) -> None: - if topic_name not in self._publishers: - if msg_type is not None: - log.info(f"Creating {topic_name} publisher") - self._publishers[topic_name] = self._node.create_publisher(msg_type, topic_name, qos_profile) - else: - log.warning(f"Unknown message type '{msg_type}'") - - def create_service_client(self, srv_name: str, srv_type: Any) -> None: - if srv_name not in self._service_clients: - if srv_type is not None: - log.info(f"Creating {srv_name} service client") - self._service_clients[srv_name] = self._node.create_client(srv_type, srv_name) - else: - log.warning(f"Unknown service type '{srv_type}") - def call_async_srv(self, srv_name: str, req: Any) -> None: - log.info(f"Calling service {srv_name}") - future = self._service_clients[srv_name].call_async(req) - return future.result() - - def publish(self, topic_name: str, msg: Any) -> None: - if topic_name in self._publishers: - publisher = self._publishers[topic_name] - publisher.publish(msg) - else: - log.warning(f"No publisher found for topic '{topic_name}'") - - def create_subscriber(self, topic_name: str, msg_type: Any, callback=None, qos_profile: int = QOS_PROFILE) -> None: - if topic_name not in self._subscribers: - if msg_type is not None: - if callback is None: - callback = self._default_callback - - log.info(f"Creating {topic_name} subscriber") - self._subscribers[topic_name] = self._node.create_subscription(msg_type, topic_name, callback, qos_profile) - else: - log.warning(f"Unknown message type '{msg_type}'") - - def _default_callback(self, msg) -> None: - log.info(f"[Default callback] Received message: {msg}") - - def create_timer(self, timer_name, period, callback) -> None: - if timer_name not in self._timers: - if callback is not None: - log.info(f"Creating {timer_name} timer") - self._timers[timer_name] = self._node.create_timer(period, callback) - else: - log.error(f"No callback function given for timer {timer_name}") diff --git a/rae-default/src/app.py b/rae-default/src/app.py index b9474cf..1b4af7a 100644 --- a/rae-default/src/app.py +++ b/rae-default/src/app.py @@ -1,13 +1,12 @@ import robothub import time import depthai as dai -import logging +import logging as log from time import sleep import threading -from src.robot.robot import Robot -from src.utils.logging import Log +from rae_sdk.robot import Robot def build_pipeline(streams): pipeline = dai.Pipeline() @@ -53,10 +52,8 @@ def __init__(self): super().__init__() self.device = None self.stream_handles = {} - self.available_apps = [] self.mutex=threading.Lock() - self.logger = Log() - self.robot = Robot(self.logger) + self.robot = None def on_start(self): if not robothub.DEVICES: @@ -66,12 +63,12 @@ def on_start(self): "click the \"Reassign devices\" button and select a device." ) self._stop(1) - self.logger.info("Starting the app...") + log.info("Starting the app...") sleep(2) - self.logger.info("Starting streams...") + log.info("Starting streams...") self.init_streams() - self.logger.info("Starting Robot") - self.robot.start() + log.info("Starting Robot") + self.robot = Robot() def on_stop(self): self.logger.info("Stopping the app...") diff --git a/rae-default/src/robot/__init__.py b/rae-default/src/robot/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rae-default/src/robot/movement.py b/rae-default/src/robot/movement.py deleted file mode 100644 index b6edd91..0000000 --- a/rae-default/src/robot/movement.py +++ /dev/null @@ -1,14 +0,0 @@ -from geometry_msgs.msg import Twist -class MovementController: - def __init__(self, ros_manager): - self.ros_manager = ros_manager - self.ros_manager.create_publisher("/cmd_vel", Twist) - def move(self, linear, angular): - twist_msg = Twist() - twist_msg.linear.x = float(linear) - twist_msg.linear.y = 0.0 - twist_msg.linear.z = 0.0 - twist_msg.angular.x = 0.0 - twist_msg.angular.y = 0.0 - twist_msg.angular.z = float(angular) - self.ros_manager.publish('/cmd_vel', twist_msg) \ No newline at end of file diff --git a/rae-default/src/robot/robot.py b/rae-default/src/robot/robot.py deleted file mode 100644 index a825ba2..0000000 --- a/rae-default/src/robot/robot.py +++ /dev/null @@ -1,81 +0,0 @@ -from typing import Optional, List, Tuple - -from src.api.ros.ros2_manager import ROS2Manager -from src.robot.movement import MovementController -from sensor_msgs.msg import BatteryState - - -class Robot: - """ - A class representing a robot, integrating various controllers for movement, display, and LED management, - and interfacing with ROS2 for communication and control. - - Attributes: - logger: An instance used for logging messages and errors. - ros_manager (ROS2Manager): An object for managing ROS2 communications and functionalities. - battery_state (BatteryState): Stores the current state of the robot's battery. - movement_controller (MovementController): Handles the robot's movement. - - Methods: - battery_state_cb(data): Callback method for updating battery state. - start(): Initializes the robot's components and starts ROS2 communications. - stop(): Stops the ROS2 communications and shuts down the robot's components. - move(velocity): Commands the robot to move at a specified velocity. - get_battery(): Retrieves the current battery state. - """ - - def __init__(self, logger): - """ - Initializes the Robot instance. - - Args: - logger: An instance used for logging messages and errors. - """ - - self.logger = logger - self.ros_manager = None - self.battery_state = None - self.movement_controller = None - - def battery_state_cb(self, data): - self.battery_state = data - - def start(self): - """ - Initializes and starts the robot's components and ROS2 communications. - Sets up necessary controllers and subscribers for the robot's functionalities. - """ - self.ros_manager = ROS2Manager("base_container") - self.ros_manager.start() - self.movement_controller = MovementController(self.ros_manager) - self.ros_manager.create_subscriber( - "/battery_status", BatteryState, self.battery_state_cb) - - def stop(self): - """ - Stops the ROS2 communications and deactivates the robot's controllers. - Ensures a clean shutdown of all components. - """ - self.display_controller.stop() - self.ros_manager.stop() - - def move(self, linear, angular): - """ - Commands the robot to move at the specified velocity. - - Args: - linear: Linear velocity. - angular: Angular velocity. - """ - self.movement_controller.move(linear, angular) - - - - def get_battery(self): - """ - Retrieves the current state of the robot's battery. - - Returns: - BatteryState: The current state of the battery. - """ - return self.battery_state diff --git a/rae-default/src/utils/__init__.py b/rae-default/src/utils/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rae-default/src/utils/logging.py b/rae-default/src/utils/logging.py deleted file mode 100644 index ba433be..0000000 --- a/rae-default/src/utils/logging.py +++ /dev/null @@ -1,14 +0,0 @@ -import logging - -class Log: - def __init__(self): - self.logger = logging.getLogger(__name__) - self.handler = logging.FileHandler("./logs/app.log") - self.formatter = logging.Formatter("%(asctime)s %(name)s %(levelname)s: %(message)s") - self.handler.setFormatter(self.formatter) - self.logger.addHandler(self.handler) - - def info(self, msg: str): - self.logger.info(msg) - def error(self, msg: str): - self.logger.error(msg) \ No newline at end of file diff --git a/rae-default/start_ros.sh b/rae-default/start_ros.sh deleted file mode 100644 index 57d583d..0000000 --- a/rae-default/start_ros.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/bash -set -e - -# setup ros environment -source "/opt/ros/$ROS_DISTRO/setup.bash" -source "/ws/install/setup.bash" - -export ROS_DOMAIN_ID=30 # setup domain id -ros2 launch rae_hw control.launch.py enable_battery_status:=false # spustil launch From 4f71ac68f347805f1407e56c164f557c1143b752 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 15 Jan 2024 12:18:16 +0100 Subject: [PATCH 2/2] minor fixes --- rae-default/robotapp.toml | 2 +- rae-default/src/app.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rae-default/robotapp.toml b/rae-default/robotapp.toml index b48f235..0f8ec83 100644 --- a/rae-default/robotapp.toml +++ b/rae-default/robotapp.toml @@ -16,4 +16,4 @@ redirectToIndex = true [runtime.runs_on] type = "image" -name = "pull ghcr.io/luxonis/rae-ros:v0.2.0-humble" +name = "ghcr.io/luxonis/rae-ros:v0.2.0-humble" diff --git a/rae-default/src/app.py b/rae-default/src/app.py index 1b4af7a..2d06680 100644 --- a/rae-default/src/app.py +++ b/rae-default/src/app.py @@ -57,7 +57,7 @@ def __init__(self): def on_start(self): if not robothub.DEVICES: - self.logger.error( + log.error( "The default app requires an assigned device to run. " "Please go to RobotHub, navigate to the app's page, " "click the \"Reassign devices\" button and select a device." @@ -71,10 +71,10 @@ def on_start(self): self.robot = Robot() def on_stop(self): - self.logger.info("Stopping the app...") + log.info("Stopping the app...") self.robot.stop() if self.device: - self.logger.info("Closing device") + log.info("Closing device") self.device.close() def init_streams(self):