Webサービス向けのバックエンドサーバーの追加

.github/workflows/custom_dict.json

@@ -121,6 +121,9 @@
     "BLAUE",
     "Hackentrick",
     "3rdparty",
+    "fastapi",
+    "uvicorn",
+    "pydantic",
     "PROTOS",
     "libprotobuf",
     "RELWITHDEBINFO",

crane_robot_command_bridge/crane_robot_command_bridge/bridge_node.py

@@ -0,0 +1,209 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+import uvicorn
+from fastapi import FastAPI, HTTPException
+from .models import *  # Pydanticモデルをインポート
+import threading
+
+
+# ROS 2メッセージの型をインポート
+from crane_msgs.msg import (
+    RobotCommand,
+    RobotCommands,
+    PositionTargetMode,
+    VelocityTargetMode,
+    SimpleVelocityTargetMode,
+)
+
+
+class ROS2Bridge(Node):
+    def __init__(self):
+        super().__init__("ros2_bridge")
+
+        # QoSプロファイルの設定
+        qos_profile = rclpy.node.QoSProfile(
+            depth=10, reliability=rclpy.qos.ReliabilityPolicy.RELIABLE
+        )
+
+        # パブリッシャーの初期化（ロボットの数に応じて動的に作成）
+        self.publisher = self.create_publisher(
+            RobotCommands, "/control_targets", qos_profile
+        )
+        self.robot_commands = {}
+
+        # ステータス更新用のタイマー
+        self.create_timer(0.1, self.timer_callback)  # 10Hz
+
+    def convert_to_ros_message(self, command_model: RobotCommandModel) -> RobotCommand:
+        """PydanticモデルからROS 2メッセージに変換"""
+        msg = RobotCommand()
+
+        # 基本フィールドの設定
+        msg.robot_id = command_model.robot_id
+        msg.local_goalie_enable = command_model.local_goalie_enable
+        msg.enable_ball_centering_control = command_model.enable_ball_centering_control
+        msg.chip_enable = command_model.chip_enable
+        msg.stop_flag = command_model.stop_flag
+        msg.lift_up_dribbler_flag = command_model.lift_up_dribbler_flag
+        msg.kick_power = command_model.kick_power
+        msg.dribble_power = command_model.dribble_power
+        msg.enable_local_feedback = command_model.enable_local_feedback
+        msg.target_theta = command_model.target_theta
+        msg.omega_limit = command_model.omega_limit
+        msg.theta_tolerance = command_model.theta_tolerance
+        msg.latency_ms = command_model.latency_ms
+        msg.elapsed_time_ms_since_last_vision = (
+            command_model.elapsed_time_ms_since_last_vision
+        )
+        msg.skill_name = command_model.skill_name
+
+        # LocalPlannerConfigの設定
+        planner_config = command_model.local_planner_config
+        msg.local_planner_config.disable_collision_avoidance = (
+            planner_config.disable_collision_avoidance
+        )
+        msg.local_planner_config.disable_goal_area_avoidance = (
+            planner_config.disable_goal_area_avoidance
+        )
+        msg.local_planner_config.disable_placement_avoidance = (
+            planner_config.disable_placement_avoidance
+        )
+        msg.local_planner_config.disable_ball_avoidance = (
+            planner_config.disable_ball_avoidance
+        )
+        msg.local_planner_config.disable_rule_area_avoidance = (
+            planner_config.disable_rule_area_avoidance
+        )
+        msg.local_planner_config.max_acceleration = planner_config.max_acceleration
+        msg.local_planner_config.max_velocity = planner_config.max_velocity
+        msg.local_planner_config.terminal_velocity = planner_config.terminal_velocity
+        msg.local_planner_config.priority = planner_config.priority
+
+        # Pose2Dの設定
+        # msg.current_velocity.x = command_model.current_velocity.x
+        # msg.current_velocity.y = command_model.current_velocity.y
+        # msg.current_velocity.theta = command_model.current_velocity.theta
+        #
+        # msg.current_pose.x = command_model.current_pose.x
+        # msg.current_pose.y = command_model.current_pose.y
+        # msg.current_pose.theta = command_model.current_pose.theta
+
+        # コントロールモードに応じたターゲットモードの設定
+        if command_model.control_mode == "POSITION_TARGET_MODE":
+            msg.control_mode = RobotCommand.POSITION_TARGET_MODE
+            if command_model.position_target_mode:
+                target = PositionTargetMode()
+                target.target_x = command_model.position_target_mode.target_x
+                target.target_y = command_model.position_target_mode.target_y
+                target.position_tolerance = (
+                    command_model.position_target_mode.position_tolerance
+                )
+                target.speed_limit_at_target = (
+                    command_model.position_target_mode.speed_limit_at_target
+                )
+                msg.position_target_mode = [target]
+
+        elif command_model.control_mode == "VELOCITY_TARGET_MODE":
+            msg.control_mode = RobotCommand.VELOCITY_TARGET_MODE
+            if command_model.velocity_target_mode:
+                target = VelocityTargetMode()
+                target.target_vx = command_model.velocity_target_mode.target_vx
+                target.target_vy = command_model.velocity_target_mode.target_vy
+                target.traj_origin_x = command_model.velocity_target_mode.traj_origin_x
+                target.traj_origin_y = command_model.velocity_target_mode.traj_origin_y
+                target.traj_origin_angle = (
+                    command_model.velocity_target_mode.traj_origin_angle
+                )
+                target.traj_curvature = (
+                    command_model.velocity_target_mode.traj_curvature
+                )
+                msg.velocity_target_mode = [target]
+
+        elif command_model.control_mode == "SIMPLE_VELOCITY_TARGET_MODE":
+            msg.control_mode = RobotCommand.SIMPLE_VELOCITY_TARGET_MODE
+            if command_model.simple_velocity_target_mode:
+                target = SimpleVelocityTargetMode()
+                target.target_vx = command_model.simple_velocity_target_mode.target_vx
+                target.target_vy = command_model.simple_velocity_target_mode.target_vy
+                target.speed_limit_at_target = (
+                    command_model.simple_velocity_target_mode.speed_limit_at_target
+                )
+                msg.simple_velocity_target_mode = [target]
+
+        return msg
+
+    def publish_command(self, robot_id: int, command: RobotCommandModel):
+        """コマンドをパブリッシュ"""
+        ros_msg = self.convert_to_ros_message(command)
+
+        commands_msg = RobotCommands()
+        commands_msg.robot_commands = [ros_msg]
+        commands_msg.header.stamp = self.get_clock().now().to_msg()
+        commands_msg.header.frame_id = "world"
+        commands_msg.on_positive_half = False
+        commands_msg.is_yellow = False
+
+        self.publisher.publish(commands_msg)
+        self.robot_commands[robot_id] = command
+
+    def timer_callback(self):
+        """定期的なステータス更新"""
+        # ここでロボットの状態を監視・更新
+        pass
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    # FastAPIアプリケーションの作成
+    app = FastAPI()
+    bridge = ROS2Bridge()
+
+    # ROSノードのスピンを別スレッドで実行
+    ros_thread = threading.Thread(target=lambda: rclpy.spin(bridge))
+    ros_thread.daemon = True
+    ros_thread.start()
+
+    # FastAPIエンドポイントの設定
+    @app.post("/api/robots/{robot_id}/command")
+    async def set_robot_command(robot_id: int, command: RobotCommandModel):
+        try:
+            if command.robot_id != robot_id:
+                raise HTTPException(status_code=400, detail="Robot ID mismatch")
+            bridge.publish_command(robot_id, command)
+            return {"status": "success"}
+        except Exception as e:
+            raise HTTPException(status_code=500, detail=str(e))
+
+    @app.get("/api/robots/{robot_id}/command")
+    async def get_robot_command(robot_id: int):
+        command = bridge.robot_commands.get(robot_id)
+        if command is None:
+            raise HTTPException(status_code=404, detail="Robot command not found")
+        return command
+
+    @app.post("/api/robots/{robot_id}/emergency-stop")
+    async def emergency_stop(robot_id: int):
+        try:
+            command = bridge.robot_commands.get(robot_id)
+            if command:
+                command.stop_flag = True
+                bridge.publish_command(robot_id, command)
+            return {"status": "success"}
+        except Exception as e:
+            raise HTTPException(status_code=500, detail=str(e))
+
+    bridge.declare_parameter("web_server_host", "localhost")
+    bridge.declare_parameter("web_server_port", 8000)
+
+    # Web serverの起動
+    uvicorn.run(
+        app,
+        host=bridge.get_parameter("web_server_host").value,
+        port=bridge.get_parameter("web_server_port").value,
+    )
+
+
+if __name__ == "__main__":
+    main()

crane_robot_command_bridge/crane_robot_command_bridge/models.py

@@ -0,0 +1,56 @@
+from pydantic import BaseModel, Field
+from typing import Optional
+
+
+# FastAPI用のPydanticモデル
+class LocalPlannerConfigModel(BaseModel):
+    disable_collision_avoidance: bool = False
+    disable_goal_area_avoidance: bool = False
+    disable_placement_avoidance: bool = False
+    disable_ball_avoidance: bool = False
+    disable_rule_area_avoidance: bool = False
+    max_acceleration: float = 4.0
+    max_velocity: float = 6.0
+    terminal_velocity: float = 0.0
+    priority: int = 0
+
+
+class Pose2DModel(BaseModel):
+    x: float = 0.0
+    y: float = 0.0
+    theta: float = 0.0
+
+
+class PositionTargetModeModel(BaseModel):
+    target_x: float
+    target_y: float
+    position_tolerance: float = 0.0
+    speed_limit_at_target: float = 0.0
+
+
+class SimpleVelocityTargetModeModel(BaseModel):
+    target_vx: float
+    target_vy: float
+    speed_limit_at_target: float = 0.0
+
+
+class RobotCommandModel(BaseModel):
+    robot_id: int
+    local_goalie_enable: bool = False
+    enable_ball_centering_control: bool = False
+    local_planner_config: LocalPlannerConfigModel
+    chip_enable: bool = False
+    stop_flag: bool = False
+    lift_up_dribbler_flag: bool = False
+    kick_power: float = Field(0.0, ge=0.0, le=1.0)
+    dribble_power: float = Field(0.0, ge=0.0, le=1.0)
+    enable_local_feedback: bool = False
+    target_theta: float = 0.0
+    omega_limit: float = 100.0
+    theta_tolerance: float = 0.0
+    latency_ms: float = 0.0
+    elapsed_time_ms_since_last_vision: int = 0
+    control_mode: str
+    skill_name: str = "API server"
+    position_target_mode: Optional[PositionTargetModeModel] = None
+    simple_velocity_target_mode: Optional[SimpleVelocityTargetModeModel] = None

crane_robot_command_bridge/launch/bridge.launch.py

@@ -0,0 +1,16 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            Node(
+                package="crane_robot_command_bridge",
+                executable="bridge_node",
+                name="crane_robot_command_bridge",
+                parameters=[{"web_server_port": 8000}, {"web_server_host": "0.0.0.0"}],
+                output="screen",
+            )
+        ]
+    )

crane_robot_command_bridge/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>crane_robot_command_bridge</name>
+  <version>0.0.0</version>
+  <description>Web API bridge for RoboCup SSL robots</description>
+  <maintainer email="[email protected]">hans</maintainer>
+  <license>MIT</license>
+
+  <depend>crane_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclpy</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

crane_robot_command_bridge/requirements.txt

@@ -0,0 +1,4 @@
+fastapi>=0.68.0
+uvicorn>=0.15.0
+pydantic>=1.8.0
+python-multipart>=0.0.5

crane_robot_command_bridge/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/crane_robot_command_bridge
+[install]
+install_scripts=$base/lib/crane_robot_command_bridge

crane_robot_command_bridge/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = "crane_robot_command_bridge"
+
+setup(
+    name=package_name,
+    version="0.0.1",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Your Name",
+    maintainer_email="[email protected]",
+    description="Web API bridge for RoboCup SSL robots",
+    license="Apache License 2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "bridge_node = crane_robot_command_bridge.bridge_node:main",
+        ],
+    },
+)