Patrol package

turtlebro_actions/turtlebro_actions/servers/move_server.py

@@ -123,7 +123,15 @@ def goal_callback(self, goal_request: Move.Goal) -> GoalResponse:
         return GoalResponse.ACCEPT
 
     def cancel_callback(self, goal_handle) -> CancelResponse:
+        with self._active_goal_lock:
+            active_goal = self._active_goal
+        if active_goal is not None and active_goal.goal_handle is not goal_handle:
+            self.get_logger().warning('Запрос отмены перемещения отклонен: активна другая цель')
+            return CancelResponse.REJECT
+
         self.get_logger().info('Получен запрос на отмену перемещения')
+        if active_goal is not None:
+            self.cmd_vel.publish(Twist())
         return CancelResponse.ACCEPT
 
     def execute_callback(self, goal_handle):

turtlebro_actions/turtlebro_actions/servers/rotate_server.py

@@ -65,15 +65,17 @@ def __init__(self) -> None:
         self.odom = Odometry()
         self._odom_received = False
         self._odom_event = threading.Event()
+        self._control_period_seconds = 1.0 / 40.0
+        self._odom_period_seconds = 0.1
         self._last_odom_time = time.monotonic()
 
         self._active_goal_lock = threading.Lock()
         self._active_goal: _RotationGoalContext | None = None
-
-        self.cmd_vel = None
-        self._odom_subscription = None
-        self._control_timer = None
-        self._resources_ready = False
+        self._control_timer = self.create_timer(
+            self._control_period_seconds,
+            self._control_step,
+            callback_group=self._callback_group,
+        )
 
         self._action_server = ActionServer(
             self,
@@ -108,7 +110,11 @@ def subscriber_odometry_cb(self, msg: Odometry) -> None:
         self.odom = msg
         self._odom_received = True
         self._odom_event.set()
-        self._last_odom_time = time.monotonic()
+        now = time.monotonic()
+        odom_dt = now - self._last_odom_time
+        if 0.001 <= odom_dt <= 1.0:
+            self._odom_period_seconds = 0.8 * self._odom_period_seconds + 0.2 * odom_dt
+        self._last_odom_time = now
 
     def goal_callback(self, goal_request: Rotation.Goal) -> GoalResponse:
         with self._active_goal_lock:
@@ -121,7 +127,15 @@ def goal_callback(self, goal_request: Rotation.Goal) -> GoalResponse:
         return GoalResponse.ACCEPT
 
     def cancel_callback(self, goal_handle) -> CancelResponse:
+        with self._active_goal_lock:
+            active_goal = self._active_goal
+        if active_goal is not None and active_goal.goal_handle is not goal_handle:
+            self.get_logger().warning('Запрос отмены поворота отклонен: активна другая цель')
+            return CancelResponse.REJECT
+
         self.get_logger().info('Получен запрос на отмену поворота')
+        if active_goal is not None:
+            self.cmd_vel.publish(Twist())
         return CancelResponse.ACCEPT
 
     def execute_callback(self, goal_handle):
@@ -225,6 +239,8 @@ def _control_step(self) -> None:
             context.max_speed,
             min_speed=context.min_speed,
         )
+        remaining_angle_rad = max(context.total_angle_rad - context.accumulated_radians, 0.0)
+        speed = min(speed, self._max_speed_for_remaining_angle(remaining_angle_rad))
         cmd = Twist()
         cmd.angular.z = context.direction * speed
         self.cmd_vel.publish(cmd)
@@ -284,6 +300,13 @@ def _wait_for_odom(self, timeout: float) -> bool:
             return self._odom_received
         return False
 
+    def _max_speed_for_remaining_angle(self, remaining_angle_rad: float) -> float:
+        if remaining_angle_rad <= 0.0:
+            return 0.0
+        control_period = max(getattr(self, '_control_period_seconds', 1.0 / 40.0), 0.001)
+        odom_period = max(getattr(self, '_odom_period_seconds', control_period), control_period)
+        return remaining_angle_rad / odom_period
+
 
 def main(args=None) -> None:
     rclpy.init(args=args)

turtlebro_actions/turtlebro_actions/servers/text_to_speech_server.py

@@ -18,6 +18,7 @@
 
 from __future__ import annotations
 
+import threading
 import time
 from typing import Optional
 
@@ -40,6 +41,10 @@ class TextToSpeechServer(Node):
     def __init__(self) -> None:
         super().__init__('text_to_speech_server')
         self._speech_client: Optional['speechd.SSIPClient'] = None
+        self._active_goal_lock = threading.Lock()
+        self._active_goal_handle = None
+
+        self._init_speech_client()
 
         self._action_server = ActionServer(
             self,
@@ -85,7 +90,10 @@ def _ensure_speech_client(self) -> None:
             self._init_speech_client()
 
     def goal_callback(self, goal_request: TextToSpeech.Goal) -> GoalResponse:
-        self._ensure_speech_client()
+        with self._active_goal_lock:
+            if self._active_goal_handle is not None:
+                self.get_logger().warning('Уже выполняется синтез речи, отклоняю новую цель')
+                return GoalResponse.REJECT
         if self._speech_client is None:
             self.get_logger().error('Запрос синтеза отклонен: speech-dispatcher недоступен')
             return GoalResponse.REJECT
@@ -95,8 +103,15 @@ def goal_callback(self, goal_request: TextToSpeech.Goal) -> GoalResponse:
         return GoalResponse.ACCEPT
 
     def cancel_callback(self, goal_handle) -> CancelResponse:
+        with self._active_goal_lock:
+            active_goal_handle = self._active_goal_handle
+        if active_goal_handle is not None and active_goal_handle is not goal_handle:
+            self.get_logger().warning('Запрос отмены синтеза отклонен: активна другая цель')
+            return CancelResponse.REJECT
+
         self.get_logger().info('Получен запрос на отмену синтеза речи')
-        self._stop_synthesis()
+        if active_goal_handle is not None:
+            self._stop_synthesis()
         return CancelResponse.ACCEPT
 
     def execute_callback(self, goal_handle):
@@ -105,47 +120,53 @@ def execute_callback(self, goal_handle):
 
         result = TextToSpeech.Result()
         feedback = TextToSpeech.Feedback()
-
-        if self._speech_client is None:
-            result.success = False
-            result.message = 'speech-dispatcher недоступен'
-            goal_handle.abort()
-            return result
+        with self._active_goal_lock:
+            self._active_goal_handle = goal_handle
 
         try:
-            self._apply_goal_settings(goal)
-            feedback.state = 'queued'
-            goal_handle.publish_feedback(feedback)
-
-            self._speech_client.speak(goal.text)
-            feedback.state = 'speaking'
-            goal_handle.publish_feedback(feedback)
+            if self._speech_client is None:
+                result.success = False
+                result.message = 'speech-dispatcher недоступен'
+                goal_handle.abort()
+                return result
 
-            completed = self._wait_until_done(goal_handle, text=goal.text)
-            if goal_handle.is_cancel_requested:
+            try:
+                self._apply_goal_settings(goal)
+                feedback.state = 'queued'
+                goal_handle.publish_feedback(feedback)
+
+                self._speech_client.speak(goal.text)
+                feedback.state = 'speaking'
+                goal_handle.publish_feedback(feedback)
+
+                completed = self._wait_until_done(goal_handle, text=goal.text)
+                if goal_handle.is_cancel_requested:
+                    self._stop_synthesis()
+                    goal_handle.canceled()
+                    result.success = False
+                    result.message = 'Синтез отменен'
+                    return result
+
+                if not completed:
+                    self.get_logger().debug('Таймаут ожидания окончания синтеза речи')
+
+                feedback.state = 'completed'
+                goal_handle.publish_feedback(feedback)
+                goal_handle.succeed()
+                result.success = True
+                result.message = 'Синтез завершен'
+                return result
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f'Ошибка синтеза речи: {exc}')
                 self._stop_synthesis()
-                goal_handle.canceled()
                 result.success = False
-                result.message = 'Синтез отменен'
+                result.message = str(exc)
+                goal_handle.abort()
                 return result
-
-            if not completed:
-                self.get_logger().debug('Таймаут ожидания окончания синтеза речи')
-
-            feedback.state = 'completed'
-            goal_handle.publish_feedback(feedback)
-            goal_handle.succeed()
-            result.success = True
-            result.message = 'Синтез завершен'
-            return result
-        except Exception as exc:  # noqa: BLE001
-            self.get_logger().error(f'Ошибка синтеза речи: {exc}')
-            self._stop_synthesis()
-            self._close_speech_client()
-            result.success = False
-            result.message = str(exc)
-            goal_handle.abort()
-            return result
+        finally:
+            with self._active_goal_lock:
+                if self._active_goal_handle is goal_handle:
+                    self._active_goal_handle = None
 
     def _apply_goal_settings(self, goal: TextToSpeech.Goal) -> None:
         if self._speech_client is None:

turtlebro_patrol/CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.10)
+project(turtlebro_patrol)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME}_node
+  PACKAGE_DIR ${PROJECT_NAME}_node
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/PatrolPoint.msg"
+  "srv/PatrolPointCallback.srv"
+  "srv/PatrolControlCallback.srv"
+  DEPENDENCIES std_msgs
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY data
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  turtlebro_patrol_node/patrol_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(ament_cmake_copyright REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_pep257 REQUIRED)
+  find_package(ament_cmake_xmllint REQUIRED)
+
+  ament_add_pytest_test(test_patrol_node test/test_patrol_node.py)
+  ament_copyright()
+  ament_lint_cmake()
+  ament_pep257()
+  ament_xmllint()
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

turtlebro_patrol/README.md

@@ -0,0 +1,60 @@
+# turtlebro_patrol (ROS 2)
+
+Пакет патрулирования для TurtleBro под ROS 2.
+
+## Что делает
+
+- Принимает команды через сервис `/patrol_control`.
+- Отправляет цели в `navigate_to_pose` (Nav2).
+- Публикует достигнутую точку в `/patrol_control/reached`.
+- Может вызывать callback-сервис в каждой точке (если задан `point_callback_service`).
+
+## Команды управления
+
+Сервис: `/patrol_control`  
+Тип: `turtlebro_patrol/srv/PatrolControlCallback`
+
+Допустимые команды:
+
+1. `start` — начать патрульный цикл с первой точки.
+2. `pause` — остановить патрулирование (без новой цели).
+3. `resume` — продолжить с текущей точки.
+4. `home` — отправить робота в домашнюю точку.
+5. `shutdown` — остановить патруль и завершить узел.
+
+## Конфигурация точек
+
+Файл с точками:
+
+`share/turtlebro_patrol/data/goals.toml`
+
+Пример:
+
+```toml
+[home]
+pose = {x = 0, y = 0, theta = 0}
+
+[[patrolling]]
+name = "Goal1"
+pose = {x = 0.5, y = 0, theta = 45}
+```
+
+## Запуск
+
+Только узел патрулирования:
+
+```bash
+ros2 launch turtlebro_patrol patrol_run.launch.py
+```
+
+С опциональным запуском навигации:
+
+```bash
+ros2 launch turtlebro_patrol patrol.launch.py run_navigation:=true
+```
+
+С callback-сервисом точки:
+
+```bash
+ros2 launch turtlebro_patrol patrol_run.launch.py point_callback_service:=my_service_name
+```

turtlebro_patrol/data/goals.toml

@@ -0,0 +1,10 @@
+[home]
+pose = {x = 0, y = 0, theta = 0}
+
+[[patrolling]]
+name = "Goal1"
+pose = {x = 0.5, y = 0, theta = 45}
+
+[[patrolling]]
+name = "Goal2"
+pose = {x = 0, y = -0.5, theta = -45}