4
4
import uvicorn
5
5
from fastapi import FastAPI , HTTPException
6
6
from .models import * # Pydanticモデルをインポート
7
- import asyncio
8
- from concurrent .futures import ThreadPoolExecutor
9
7
import threading
10
8
11
9
12
10
# ROS 2メッセージの型をインポート
13
- from geometry_msgs .msg import Pose2D
14
11
from crane_msgs .msg import (
15
12
RobotCommand ,
16
13
RobotCommands ,
17
- LocalPlannerConfig ,
18
14
PositionTargetMode ,
19
15
VelocityTargetMode ,
20
16
SimpleVelocityTargetMode ,
@@ -31,7 +27,9 @@ def __init__(self):
31
27
)
32
28
33
29
# パブリッシャーの初期化(ロボットの数に応じて動的に作成)
34
- self .publisher = self .create_publisher (RobotCommands , "/control_targets" , qos_profile )
30
+ self .publisher = self .create_publisher (
31
+ RobotCommands , "/control_targets" , qos_profile
32
+ )
35
33
self .robot_commands = {}
36
34
37
35
# ステータス更新用のタイマー
@@ -55,7 +53,9 @@ def convert_to_ros_message(self, command_model: RobotCommandModel) -> RobotComma
55
53
msg .omega_limit = command_model .omega_limit
56
54
msg .theta_tolerance = command_model .theta_tolerance
57
55
msg .latency_ms = command_model .latency_ms
58
- msg .elapsed_time_ms_since_last_vision = command_model .elapsed_time_ms_since_last_vision
56
+ msg .elapsed_time_ms_since_last_vision = (
57
+ command_model .elapsed_time_ms_since_last_vision
58
+ )
59
59
msg .skill_name = command_model .skill_name
60
60
61
61
# LocalPlannerConfigの設定
@@ -69,7 +69,9 @@ def convert_to_ros_message(self, command_model: RobotCommandModel) -> RobotComma
69
69
msg .local_planner_config .disable_placement_avoidance = (
70
70
planner_config .disable_placement_avoidance
71
71
)
72
- msg .local_planner_config .disable_ball_avoidance = planner_config .disable_ball_avoidance
72
+ msg .local_planner_config .disable_ball_avoidance = (
73
+ planner_config .disable_ball_avoidance
74
+ )
73
75
msg .local_planner_config .disable_rule_area_avoidance = (
74
76
planner_config .disable_rule_area_avoidance
75
77
)
@@ -94,7 +96,9 @@ def convert_to_ros_message(self, command_model: RobotCommandModel) -> RobotComma
94
96
target = PositionTargetMode ()
95
97
target .target_x = command_model .position_target_mode .target_x
96
98
target .target_y = command_model .position_target_mode .target_y
97
- target .position_tolerance = command_model .position_target_mode .position_tolerance
99
+ target .position_tolerance = (
100
+ command_model .position_target_mode .position_tolerance
101
+ )
98
102
target .speed_limit_at_target = (
99
103
command_model .position_target_mode .speed_limit_at_target
100
104
)
@@ -108,8 +112,12 @@ def convert_to_ros_message(self, command_model: RobotCommandModel) -> RobotComma
108
112
target .target_vy = command_model .velocity_target_mode .target_vy
109
113
target .traj_origin_x = command_model .velocity_target_mode .traj_origin_x
110
114
target .traj_origin_y = command_model .velocity_target_mode .traj_origin_y
111
- target .traj_origin_angle = command_model .velocity_target_mode .traj_origin_angle
112
- target .traj_curvature = command_model .velocity_target_mode .traj_curvature
115
+ target .traj_origin_angle = (
116
+ command_model .velocity_target_mode .traj_origin_angle
117
+ )
118
+ target .traj_curvature = (
119
+ command_model .velocity_target_mode .traj_curvature
120
+ )
113
121
msg .velocity_target_mode = [target ]
114
122
115
123
elif command_model .control_mode == "SIMPLE_VELOCITY_TARGET_MODE" :
0 commit comments