diff --git a/.github/workflows/main.workflow.yaml b/.github/workflows/main.workflow.yaml index d4ed6a1..03396b6 100644 --- a/.github/workflows/main.workflow.yaml +++ b/.github/workflows/main.workflow.yaml @@ -15,6 +15,44 @@ env: ROS_DISTRO: humble jobs: + clang-format-lint: + name: ament_clang_format + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + steps: + - uses: actions/checkout@v3.3.0 + - uses: ros-tooling/setup-ros@0.5.0 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: ros-tooling/action-ros2-lint@0.1.3 + with: + distribution: ${{ env.ROS_DISTRO }} + linter: clang_format + arguments: --config ./.clang-format + package-name: | + rae_hw + linting: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + linter: [xmllint, pep257, lint_cmake] + steps: + - uses: actions/checkout@v3.3.0 + - uses: ros-tooling/setup-ros@0.5.0 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: ros-tooling/action-ros2-lint@0.1.3 + with: + distribution: ${{ env.ROS_DISTRO }} + linter: ${{ matrix.linter }} + package-name: | + rae_hw + robot_py + rae_description + rae_bringup docker-build: name: Build and Upload to Docker Hub runs-on: ubuntu-22.04 diff --git a/robot_py/robot_py/robot/api/ros/ros_interface.py b/robot_py/robot_py/robot/api/ros/ros_interface.py index f266a93..4243e2b 100644 --- a/robot_py/robot_py/robot/api/ros/ros_interface.py +++ b/robot_py/robot_py/robot/api/ros/ros_interface.py @@ -1,8 +1,15 @@ +# needed by RH +import sys +del sys.path[0] +sys.path.append('') + import logging as log import os import threading import rclpy import subprocess +import asyncio +import multiprocessing from time import sleep import signal from functools import partial @@ -20,11 +27,15 @@ from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener from geometry_msgs.msg import TransformStamped +from launch import LaunchDescription, LaunchService +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource QOS_PROFILE = 10 log.basicConfig(level=log.INFO) + class ROSInterface: """ A class that manages ROS2 functionalities for a robot or a system. It includes initializing ROS2 context, @@ -70,6 +81,7 @@ def __init__(self, name: str, namespace='/rae') -> None: """ self._namespace = namespace self._ros_proc = None + self._rs = None self._name = name self._context: rclpy.context.Context | None = None self._node: rclpy.node.Node | None = None @@ -88,20 +100,34 @@ def start_hardware_process(self): """ Starts RAE hardware drivers in a separate process. """ - env = dict(os.environ) - script_name = os.path.join(get_package_share_directory( - 'robot_py'), 'scripts', 'start_ros.sh') - self._ros_proc = subprocess.Popen( - f"bash -c 'chmod +x {script_name} ; {script_name}'", shell=True, env=env, preexec_fn=os.setsid - ) - + self._rs = LaunchService(noninteractive=True) + ld = LaunchDescription([IncludeLaunchDescription(PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('rae_hw'), 'launch', 'control.launch.py')))]) + self._stop_event = multiprocessing.Event() + self._process = multiprocessing.Process(target=self._run_process, args=(self._stop_event, ld), daemon=True) + self._process.start() + print('test') + # env = dict(os.environ) + # script_name = os.path.join(get_package_share_directory( + # 'robot_py'), 'scripts', 'start_ros.sh') + # self._ros_proc = subprocess.Popen( + # f"bash -c 'chmod +x {script_name} ; {script_name}'", shell=True, env=env, preexec_fn=os.setsid + # ) + def _run_process(self, stop_event, launch_description): + loop = asyncio.get_event_loop() + launch_service = LaunchService() + launch_service.include_launch_description(launch_description) + launch_task = loop.create_task(launch_service.run_async()) + loop.run_until_complete(loop.run_in_executor(None, stop_event.wait)) + if not launch_task.done(): + asyncio.ensure_future(launch_service.shutdown(), loop=loop) + loop.run_until_complete(launch_task) def start(self, start_hardware) -> 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. """ if start_hardware: self.start_hardware_process() - sleep(2) self._context = rclpy.Context() self._context.init() log.info("ROS2 context initialized.") @@ -131,9 +157,8 @@ 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) + self._stop_event.set() + self._process.join() def stop(self) -> None: """ @@ -177,7 +202,7 @@ def _destroy_interfaces(self): for timer_name, timer in self._timers.items(): log.info(f"Destroying {timer_name} timer...") self._node.destroy_timer(timer) - + for action_name, action_client in self._action_clients.items(): log.info(f"Destroying {action_name} action client...") action_client['goal_handle'].cancel_goal_async() @@ -254,35 +279,39 @@ def create_action_client(self, action_name: str, action_type: Any) -> None: else: log.warning(f"Unknown action type '{action_type}'") - def call_async_action_simple(self, action_name: str, goal: Any)-> ClientGoalHandle: + def call_async_action_simple(self, action_name: str, goal: Any) -> ClientGoalHandle: log.info(f"Calling action {action_name}") self._action_clients[action_name]['client'].wait_for_server() - future = self._action_clients[action_name]['client'].send_goal_async(goal) + future = self._action_clients[action_name]['client'].send_goal_async( + goal) while not future.done(): log.info('Waiting for result...') sleep(0.5) return future.result() - - def call_async_action(self, action_name: str, goal: Any, goal_response_callback=None, goal_result_callback=None, goal_feedback_callback=None)-> ClientGoalHandle: + + def call_async_action(self, action_name: str, goal: Any, goal_response_callback=None, goal_result_callback=None, goal_feedback_callback=None) -> ClientGoalHandle: log.info(f"Calling action {action_name}") if goal_response_callback is None: - self._action_clients[action_name]['goal_response_callback']=self._default_goal_response_callback + self._action_clients[action_name]['goal_response_callback'] = self._default_goal_response_callback else: - self._action_clients[action_name]['goal_response_callback']=goal_response_callback + self._action_clients[action_name]['goal_response_callback'] = goal_response_callback if goal_result_callback is None: - self._action_clients[action_name]['goal_result_callback']=self._default_goal_result_callback + self._action_clients[action_name]['goal_result_callback'] = self._default_goal_result_callback else: - self._action_clients[action_name]['goal_result_callback']=goal_result_callback + self._action_clients[action_name]['goal_result_callback'] = goal_result_callback self._action_clients[action_name]['client'].wait_for_server() if goal_feedback_callback is None: - future = self._action_clients[action_name]['client'].send_goal_async(goal) + future = self._action_clients[action_name]['client'].send_goal_async( + goal) else: - future = self._action_clients[action_name]['client'].send_goal_async(goal, goal_feedback_callback) + future = self._action_clients[action_name]['client'].send_goal_async( + goal, goal_feedback_callback) + + future.add_done_callback( + partial(self._action_clients[action_name]['goal_response_callback'])) - future.add_done_callback(partial(self._action_clients[action_name]['goal_response_callback'])) - def _default_goal_response_callback(self, future): action_name = '/fibonacci' self._action_clients[action_name]['goal_handle'] = future.result() @@ -290,8 +319,10 @@ def _default_goal_response_callback(self, future): log.info('Goal rejected :(') return log.info('Goal accepted :)') - get_result_future = self._action_clients[action_name]['goal_handle'].get_result_async() - get_result_future.add_done_callback(partial(self._action_clients[action_name]['goal_result_callback'])) + get_result_future = self._action_clients[action_name]['goal_handle'].get_result_async( + ) + get_result_future.add_done_callback( + partial(self._action_clients[action_name]['goal_result_callback'])) def _default_goal_result_callback(self, future): result = future.result().result @@ -301,7 +332,7 @@ def cancel_action(self, action_name: str): log.info(f"Cancelling action {action_name}") self._action_clients[action_name]['goal_handle'].cancel_goal_async() - def get_frame_position(self, source_frame, target_frame)-> TransformStamped: + def get_frame_position(self, source_frame, target_frame) -> TransformStamped: """ Gets the position of a frame relative to another frame. diff --git a/robot_py/robot_py/robot/robot.py b/robot_py/robot_py/robot/robot.py index c0152d8..f3efc54 100644 --- a/robot_py/robot_py/robot/robot.py +++ b/robot_py/robot_py/robot/robot.py @@ -29,28 +29,17 @@ class Robot: stop(): Stops the ROS2 communications and shuts down the robot's components. """ - def __init__(self, name='rae_api', namespace='/rae'): + def __init__(self, start_hardware=True, name='rae_api', namespace='/rae'): """ Initializes the Robot instance. + + Args: + start_hardware (bool, optional): If True, starts the robot's hardware components. Defaults to True. + name (str, optional): The name of the ROS2 node. Defaults to 'rae_api'. + namespace (str, optional): The namespace of the ROS2 node. Defaults to '/rae'. """ - self._ros_interface = None - self._battery_state = None - self._led_controller = None - self._display_controller = None - self._movement_controller = None - self._audio_controller = None - self._perception_system = PerceptionSystem() self._name = name self._namespace = namespace - - def battery_state_cb(self, data): - self._battery_state = data - - def start(self, start_hardware=True): - """ - Initializes and starts the robot's components and ROS2 communications. - Sets up necessary controllers and subscribers for the robot's functionalities. - """ self._ros_interface = ROSInterface(self._name, self._namespace) self._ros_interface.start(start_hardware) self._led_controller = LEDController(self._ros_interface) @@ -59,19 +48,24 @@ def start(self, start_hardware=True): self._audio_controller = AudioController(self._ros_interface) self._ros_interface.create_subscriber( "/battery_status", BatteryState, self.battery_state_cb) - self._perception_system.start() + # self._perception_system = PerceptionSystem() + log.info('Robot ready') + def __del__(self) -> None: + self.stop() def stop(self): """ Stops the ROS2 communications and deactivates the robot's controllers. Ensures a clean shutdown of all components. """ - self.perception_system.stop() + # self._perception_system.stop() if self._display_controller is not None: self._display_controller.stop() self._ros_interface.stop() + def battery_state_cb(self, data): + self._battery_state = data @property def battery_state(self): """ @@ -116,4 +110,4 @@ def audio_controller(self) -> AudioController: """ Returns the robot's audio controller. """ - return self._audio_controller + return self._audio_controller \ No newline at end of file