diff --git a/.cspell.json b/.cspell.json new file mode 100644 index 00000000000..0cd761eb370 --- /dev/null +++ b/.cspell.json @@ -0,0 +1,3 @@ +{ + "words": ["ISCLOAM"] +} diff --git a/docs/design/autoware-architecture/node-diagram/overall-node-diagram-autoware-universe.drawio.svg b/docs/design/autoware-architecture/node-diagram/overall-node-diagram-autoware-universe.drawio.svg index 1aa2d3f2c54..b531addd3c9 100644 --- a/docs/design/autoware-architecture/node-diagram/overall-node-diagram-autoware-universe.drawio.svg +++ b/docs/design/autoware-architecture/node-diagram/overall-node-diagram-autoware-universe.drawio.svg @@ -1,4 +1,4 @@ -
Planning
Planning
Control
Control
Perception
Perception
Vehicle
Vehicle
Sensing
Sensing
VehicleInterface
VehicleInterface
Localization
Localization
System
System
/diagnostics
/diagnostics
XX1
XX1
X2
X2
X1
X1
S1
S1
/vehicle/status/velocity_report
/vehicle/status/velocity_report
/autoware/engage [Engage]
/current_gate_mode [GateMode]
/autoware/engage [Engage]...
/planning/turn_indicators_cmd [TurnIndicatorsCommand]
/planning/hazard_lights_cmd [HazardLightsCommand]
/planning/turn_indicators_cmd [TurnIndicatorsCommand]...
/planning/scenario_planning/max_velocity_candidates
[tier4_planning_msgs/msg/VelocityLimit]
/planning/scenario_planning/max_velocity_candidates...
HMI
HMI
HMI
HMI
HMI
HMI
/api/external/set/command/remote/control: [tier4_external_api_msgs/msg/ControlCommandStamped]
/api/external/set/command/remote/heartbeat: [tier4_external_api_msgs/msg/Heartbeat]
/api/external/set/command/remote/shift: [tier4_external_api_msgs/msg/GearShiftStamped]
/api/external/set/command/remote/turn_signal: [tier4_external_api_msgs/msg/TurnSignalStamped]
/api/external/set/command/remote/control: [tier4_external_api_msgs/msg/ControlCommandStamped]...
HMI
HMI
/vehicle/engage
/vehicle/engage
HMI
HMI
/diagnostics_err
/diagnostics_err
/initialpose
/initialpose
<pose_initializer
_srv>
<pose_initializer...
vehicle_velocity_converter/
twist_with_covariance
vehicle_velocity_converter/...
HMI
HMI
pointcloud_map_
loader
pointcloud_map_...
<differential_map_
loader_srv>
<differential_map_...
<partial_map_
loader_srv>
<partial_map_...
voxel_grid_downsample/pointcloud
voxel_grid_downsample/pointcloud
立体格子の点群密度が一様になるように点群を間引く。点群密度が正規化されるため、NDTのロバスト化に一役買っている。立体格子の点群密度が一様になるように点群を間引く。点群密度が正規化されるため、NDTのロバスト化に一役買っている。
voxel_grid_
downsample_filter
voxel_grid_...
pose_twist_fusion_filter/
pose_with_covariance_
no_yawbias
pose_twist_fusion_filter/...
Estimate ego vehicle's position, orientation, and velocity by EKF algorithm.Estimate ego vehicle's position, orientation, and velocity by EKF algorithm.
ekf_localizer
ekf_localizer
pose_estimator/
pose_with_covariance
pose_estimator/...
LiDARの点群とpointcloud_mapとのマッチングを行い、自己位置を計算するLiDARの点群とpointcloud_mapとのマッチングを行い、自己位置を計算する
ndt_scan_matcher
ndt_scan_matcher
downsample/pointcloud
downsample/pointcloud
点群数が一定以下になるようにランダムに間引く。基本的にはvoxel grid filterで丁度いいくらい点群数になるように調整してあり、点群数のlimiter的な立ち位置が強い 点群数が一定以下になるようにランダムに間引く。基本的にはvoxel grid filterで丁度いいくらい点群数になるように調整してあり、点群数のlimiter的な立ち位置が強い
random_
downsample_filter
random_...
/initialpose3d
/initialpose3d
GNSS / Userからラフな初期位置を受け取り、NDT+モンテカルロ法で正確な自己位置を推定して出力する。NDTとはserviceでやり取りする。GNSS / Userからラフな初期位置を受け取り、NDT+モンテカルロ法で正確な自己位置を推定して出力する。NDTとはserviceでやり取りする。
pose_initializer
pose_initializer
twist_estimator/twist_with_covariance
twist_estimator/twist_with_covariance
車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。
gyro_odometer
gyro_odometer
measurement_range/pointcloud
measurement_range/pointcloud
一定範囲内のLiDAR点群のみを切り取って出力する。ホントは遠くまで見たいけど、歪みの影響が大きくなったり、そもそも遠方の点群地図がなかったりするので、近くだけを見てる。一定範囲内のLiDAR点群のみを切り取って出力する。ホントは遠くまで見たいけど、歪みの影響が大きくなったり、そもそも遠方の点群地図がなかったりするので、近くだけを見てる。
crop_box_filter_
measurement_range
crop_box_filter_...
車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。
vehicle_velocity_converter
vehicle_velocity_converter
/tf
(map to base_link)
/tf...
/localization/kinematic_state
[nav_msgs/msg/Odometry]
/localization/kinematic_state...
Overwrite vx=0 and wz=0 when the vehicle wheel speed is zero to indicate the stop condition.Overwrite vx=0 and wz=0 when the vehicle wheel speed is zero to indicate the stop condition.
stop_filter
stop_filter
localization_error_monitor
localization_error_monitor
pose_twist_fusion_filter/
pose_with_covariance
pose_twist_fusion_filter/...
/diagnostics
/diagnostics
pose_twist_fusion_filter/
kinematic_state
pose_twist_fusion_filter/...
Map
Map
Check the trajectory and stop publishing it if it is unsafe.Check the trajectory and stop publishing it if it is unsafe.
planning_validator
planning_validator
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
invalid_lanelet
invalid_lanelet
/map
/vector_map
/map...
lanelet2_map_loader
lanelet2_map_loader
/map/
map_projector_info
/map/...
map_projection_
loader
map_projection_...
Perception
Perception
/perception/object_recognition/objects
/perception/object_recognition/objects
traffic_light_states
traffic_light_states
/perception/traffic_light_recognition/
/perception/traffic_light_...
/perception/object_recognition/
/perception/object_recognition/
/perception/object_recognition/detection/
/perception/object_recognition/detectio...
clustering/clusters
clustering/clusters
euclidean_cluster
euclidean_cluster
pointcloud_map_filtered/pointcloud
pointcloud...

detection_by
_tracker/objects

detection_by...
centerpoint/validation/objects
centerpoint/validation/objects
DNNベースでLiDAR点群に物体のクラス情報を付与するDNNベースでLiDAR点群に物体のクラス情報を付与する
lidar_centerpoint
lidar_centerpoint
高精度地図情報を用いて、trackingされた動物体情報の移動経路予測を行う高精度地図情報を用いて、trackingされた動物体情報の移動経路予測を行う
map_based_
prediction
map_based_...
traffic_light_detection/rough/rois
traffic_light_detection/rough/rois
自己位置および高精度地図情報、自身の走行ルート情報から、信号機のおおよその位置を計算する自己位置および高精度地図情報、自身の走行ルート情報から、信号機のおおよその位置を計算する
traffic_light_map_
based_detector
traffic_light_map_...
detection/objects
detection/objects
画像に含まれる信号機の色を識別する画像に含まれる信号機の色を識別する
traffic_light_
classifier
traffic_light_...
画像処理を用いて信号機の正確な位置を計算する画像処理を用いて信号機の正確な位置を計算する
traffic_light_
ssd_fine_detector
traffic_light_...
route,
vector_map
route,...
クラス+位置+形状情報に対してtrackingを行う。(最近上流が速度情報も出せるようになってきたらしい)クラス+位置+形状情報に対してtrackingを行う。(最近上流が速度情報も出せるようになってきたらしい)
multi_object_
tracker
multi_object_...
detection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeするdetection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeする
object_association
_merger
object_association...
clustering結果の形状を推定するclustering結果の形状を推定する
shape_estimation
shape_estimation
/perception/occupancy_grid_map/
/perception/occupancy_grid_map/
/perception/obstacle_segmentation/
/perception/obstacle_segmentation/
no_ground/oneshot/pointcloud
no_ground/oneshot/pointcloud
range_cropped/pointcloud
range_cropped/pointcloud
アルゴリズムを使って点群から地面を除去します。アルゴリズムを使って点群から地面を除去します。
common_
ground_filter
common_...
occupancy_grid
occupancy_grid
/perception/occupancy_grid_map/map
/perception/occupancy_grid_map/map
全点群データ、地面除去済み点群、occupancy gridの情報を総合的に見て、障害物が存在するかどうかの確率をoccupancy gridマップの形式で計算します。全点群データ、地面除去済み点群、occupancy gridの情報を総合的に見て、障害物が存在するかどうかの確率をoccupancy gridマップの形式で計算します。
probabilistic_
occupancy_grid_map
probabilistic_...
occupancy gridで与えられる障害物存在確率マップの情報をもとに、障害物点群をフィルタリングします。occupancy gridで与えられる障害物存在確率マップの情報をもとに、障害物点群をフィルタリングします。
occupancy_grid_
map_based_outlier_filter
occupancy_grid_...
/perception/obstacle_segmentation/pointcloud
/perception/obstacle_segmentation/pointcloud
crop_box_filter
crop_box_filter
/concatenated/pointcloud
/concatena...
BBox内に存在するobstacle_segmentation後の点群数を用いて,false positiveを除くBBox内に存在するobstacle_segmentation後の点群数を用いて,false positiveを除く
obstacle_pointcloud _based_validator
obstacle_pointcloud _ba...
detection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeするdetection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeする
object_association
_merger
object_association...
DNNベースで画像に物体のクラス情報を付与するDNNベースで画像に物体のクラス情報を付与する
tensorrt_yolo
tensorrt_yolo
/perception/obstacle_segmentation/pointcloud
/perception/obstacle_segmentation/pointcloud
clustering結果に画像のdetection結果をprojectionしてlabelを付与するclustering結果に画像のdetection結果をprojectionしてlabelを付与する
roi_cluster_fusion
roi_cluster_fusion
/rois*
/rois*
clustering結果の形状を推定するclustering結果の形状を推定する
shape_estimation
shape_estimation
tracker内部のclusterをマージし,shape fittingしたbboxを出力するtracker内部のclusterをマージし,shape fittingしたbboxを出力する
detection_by_
tracker_node
detection_by_...
clustering/camera_lidar_fusion
/objects
clustering/camera_lidar_fusion...
/sensing/camera/
camera*/image_rect_color
/sensing/camera/...
vector mapの情報を用いて,unknown objectをfilterする.lane内のunknown objectのみを残す.vector mapの情報を用いて,unknown objectをfilterする.lane内のunknown objectのみを残す.
object_lanelet_filter
object_lanelet_filter
/map/vector_map
/map/vector_map
/map/vector_map
/map/vector_map
compare_map_filter
compare_map_filter
/perception/obstacle_segmentation/pointcloud
/perception/obstacle_segmentation/pointcloud
map
map
交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。
behavior_velocity_planner
behavior_velocity_planner
Planning
Planning
/planning/scenario_planning/trajectory [Trajectory]
/planning/scenario_planning/trajectory [Trajectory]
motion_planning/obstacle_avoidance_planner/trajectory [Trajectory]
motion_planning/obstacle_avoidance_planner/trajectory [Trajectory]
車両形状や走行可能領域を考慮して、経路の平滑化を行う。障害物も避けるが、long historyで最近は回避機能はoffになっています。車両形状や走行可能領域を考慮して、経路の平滑化を行う。障害物も避けるが、long historyで最近は回避機能はoffになっています。
obstacle_
avoidance_planner
obstacle_...
motion_planning/obstacle_velocity_limiter/trajectory [Trajectory]
motion_planning/obstacle_velocity_limiter/trajectory [Trajectory]
自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。
obstacle_velocity_limiter
obstacle_velocity_limit...
障害物停止・前車追従・近傍障害物減速の複数の機能を持ち、機能に応じて経路上に速度を埋め込む。 目標経路上に障害物点がある場合は停止、経路近くに障害物点群がある場合は減速、前方に車両が居る場合は前車追従。障害物停止・前車追従・近傍障害物減速の複数の機能を持ち、機能に応じて経路上に速度を埋め込む。 目標経路上に障害物点がある場合は停止、経路近くに障害物点群がある場合は減速、前方に車両が居る場合は前車追従。
obstacle_stop_
planner
obstacle_stop_...
障害物点群や地図の走行可能エリアの情報から、駐車プランナー用のコストマップを生成する。障害物点群や地図の走行可能エリアの情報から、駐車プランナー用のコストマップを生成する。
costmap_generator
costmap_generator
apiとautoware.iv内部から送られてくる制約付き減速指示を受け取り、整合性をとるapiとautoware.iv内部から送られてくる制約付き減速指示を受け取り、整合性をとる
external_velocity_
limit_selector
external_velocity_...
/planning/scenario_planning/max_velocity
[tier4_planning_msgs/msg/VelocityLimit]
/planning/scenario_planning/max_velocity...
/planning/scenario_planning/max_velocity_candidates
[tier4_planning_msgs/msg/VelocityLimit]
/planning/scenario_planning/max_velocity_candidates...
lane_driving/trajectory [Trajectory]
lane_driving/trajectory [Trajectory]
trajectory [Trajectory]
trajectory [Trajectory]
いまのrouteおよび地図情報から、LaneDriving / Parkingのシナリオを判断し、どちらの経路を使うかを切り替える。いまのrouteおよび地図情報から、LaneDriving / Parkingのシナリオを判断し、どちらの経路を使うかを切り替える。
scenario_selector
scenario_selector
/planning/scenario_planning/motion_velocity_smoother/trajectory [Trajectory]
/planning/scenario_planning/motion_velocity_smoother/trajectory [Trajectory]
経路に埋め込まれた最大速度を超過しないように、うまい具合で速度を平滑化する。カーブでの減速もここで行う。経路に埋め込まれた最大速度を超過しないように、うまい具合で速度を平滑化する。カーブでの減速もここで行う。
motion_velocity_
smoother
motion_velocity_...
交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。
behavior_path_planner
behavior_path_planner
ルート情報や障害物情報をもとに、障害物回避を実行します。ルート情報や障害物情報をもとに、障害物回避を実行します。
obstacle_avoidance
obstacle_avoidance
ルート情報をもとに路肩への幅寄せ、路肩からの発進を行います。ルート情報をもとに路肩への幅寄せ、路肩からの発進を行います。
pull_over/out
pull_over/out
遠隔からの司令をもとに幅寄せを行います。遠隔からの司令をもとに幅寄せを行います。
side_shift
side_shift
ルート情報や障害物情報をもとに、レーンチェンジを実行します。ルート情報や障害物情報をもとに、レーンチェンジを実行します。
lane_change
lane_change
ルート情報をもとに走行経路を作成します。ルート情報をもとに走行経路を作成します。
lane_following
lane_following
<Lane Driving>
<Lane Driving>
停止禁止区域内で停止しないように、区域手前での進行・停止判断を行います。停止禁止区域内で停止しないように、区域手前での進行・停止判断を行います。
no_stopping_area
no_stopping_area
地図情報および動物体情報から、死角手前での減速を行います。地図情報および動物体情報から、死角手前での減速を行います。
occlusion_spot
occlusion_spot
地図情報および動物体情報から、交差点での停止/発進の判定を行います。地図情報および動物体情報から、交差点での停止/発進の判定を行います。
intersection
intersection
信号の色に応じて停止/発進の指示をします信号の色に応じて停止/発進の指示をします
traffic_light
traffic_light
横断歩道に人が居る、もしくは侵入しようとしてきている場合に、横断歩道手前で停止します。近くに人が居る場合は徐行します。横断歩道に人が居る、もしくは侵入しようとしてきている場合に、横断歩道手前で停止します。近くに人が居る場合は徐行します。
cross_walk
cross_walk
右左折時に車両後方の巻き込み確認を行い、衝突の危険がある場合は停止します。右左折時に車両後方の巻き込み確認を行い、衝突の危険がある場合は停止します。
blind_spot
blind_spot
規定エリア内の障害物点群がある場合に、対応する位置で停止します規定エリア内の障害物点群がある場合に、対応する位置で停止します
detection_area
detection_area
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
stop_line
stop_line
/planning/mission_planning/route
[autoware_auto_planning_msgs/HADMapRoute]
/planning/mission_planning/route...
高精度地図情報をもとに、自己位置からゴールまでのルートを計算します。高精度地図情報をもとに、自己位置からゴールまでのルートを計算します。
mission_planner
mission_planner
behavior_planning/path_with_lane_id
behavior_planning/path_with_lane_id
behavior_planning/path
behavior_planning/path
コストマップとゴールから、自車の走行ルートを計算する。いまはHA*を使ってる。コストマップとゴールから、自車の走行ルートを計算する。いまはHA*を使ってる。
freespace_planner
freespace_planner
costmap_generator/occupancy_grid
costmap_generator/occupancy_grid
goal
goal
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
no_drivable_lane
no_drivable_lane
<Parking>
<Parking>
parking/trajectory [Trajectory]
parking/trajectory [Trajectory]
自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。
surround_obstacle_checker
surround_obstacle_checker
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
run_out
run_out
信号の色に応じて停止/発進の指示をします信号の色に応じて停止/発進の指示をします
virtual_traffic_light
virtual_traffic_light
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
speed_bump
speed_bump
trajectory_follower
trajectory_follower
Control
Control
縦横を分けて計算された制御コマンドを統合して出力します縦横を分けて計算された制御コマンドを統合して出力します
latlon_muxer
latlon_muxer
 /control/trajectory_follower/lateral/control_cmd
 [AckermannLateralControlCommand]

/control/trajectory_follower/lateral/control_cmd...
/control/trajectory_follower/longitudinal/control_cmd
[LongitudinalControlCommand]
/control/trajectory_follower/longitudinal/control_cmd...
/control/trajectory_follower/control_cmd
 [AckermannControlCommand]

/control/trajectory_follower/control_cmd...
経路追従のための目標ステア角とステア角速度を計算します。目標速度と加速度は経路に埋め込まれた値をそのまま出力します。経路追従のための目標ステア角とステア角速度を計算します。目標速度と加速度は経路に埋め込まれた値をそのまま出力します。
lateral_controller
lateral_controller
経路の目標速度に沿うための目標加速度を計算します。遅延補正や停止時のブレーキ抜き操作も行います。経路の目標速度に沿うための目標加速度を計算します。遅延補正や停止時のブレーキ抜き操作も行います。
longitudinal_controller
longitudinal_controller
指示モードに応じて、複数のコマンドの中から後段に渡すコマンドを選択します。指示モードに応じて、複数のコマンドの中から後段に渡すコマンドを選択します。
vehicle_cmd_gate
vehicle_cmd_gate
control/command/control_cmd
 [AckermannControlCommand]
control/command/control_cmd...
/control/shift_decider/gear_cmd
[GearCommand]
/control/shift_decider/gear_cmd...
制御出力に応じてギア変更のコマンドを出力します制御出力に応じてギア変更のコマンドを出力します
shift_decider
shift_decider
遠隔から来たコマンド(アクセル/ブレーキ)を目標速度/加速度の信号に変換します遠隔から来たコマンド(アクセル/ブレーキ)を目標速度/加速度の信号に変換します
external_cmd_
converter
external_cmd_...
/diagnostics
/diagnostics
MPCの出す予測経路と、laneletのレーン情報を用いて、車両がレーン外に出ようとしているかどうかを判定する。diag出力をerror_monitorに送り、逸脱判断字は停止する(option)MPCの出す予測経路と、laneletのレーン情報を用いて、車両がレーン外に出ようとしているかどうかを判定する。diag出力をerror_monitorに送り、逸脱判断字は停止する(option)
lane_departure_
checker
lane_departure_...
predicted_trajectory
predicted_trajectory
/external/selected/external_control_cmd
 [tier4_external_api_msgs/msg/ControlCommandStamped]
/external/selected/external_control_cmd...
external_cmd_
selector
external_cmd_...
/api/external/set/command/remote/control 
   [tier4_external_api_msgs/msg/ControlCommandStamped]
/api/external/set/command/remote/shift
   [tier4_external_api_msgs/msg/GearShiftStamped]
/api/external/set/command/remote/turn_signal
   [tier4_external_api_msgs/msg/TurnSignalStamped]
/api/external/set/command/remote/control...
z
z
/current_gate_mode [GateMode]
/gear_cmd [GearCommand]
/turn_indicators_cmd  [TurnIndicatorCommand]
/hazard_lights_cmd  [HazardLightsCommand]
/vehicle_emergency_cmd [VehicleEmergencyStamped]
/current_gate_mode [GateMode]...
ヤマハのGCとのインターフェースヤマハのGCとのインターフェース
g30_interface
g30_interface
速度・加速度の目標値を、車両特有の制御信号(アクセル/ブレーキ等)に変換します速度・加速度の目標値を、車両特有の制御信号(アクセル/ブレーキ等)に変換します
raw_vehicle_cmd_
converter
raw_vehicle_cmd_...
/vehicle/command/actuation_cmd
/vehicle/command/actuation_cmd
pacmodとのinterfaced。LexusとかJapanTaxiとか。pacmodとのinterfaced。LexusとかJapanTaxiとか。
pacmod_interface
pacmod_interface
VehicleInterface
VehicleInterface
/vehicle/status/velocity_status
/vehicle/status/steering_status
/vehicle/status/turn_indicators_status
/vehicle/status/gear_status
/vehicle/status/control_mode
/vehicle/status/actuation_status
/vehicle/s...
Autowareと車両のinterfaceです。データの変換が主なタスクです。Autowareと車両のinterfaceです。データの変換が主なタスクです。
    vehicle_interface
    vehicle_interface
accel_map_calibrator
accel_map_calibrator
/pacmod/**
/pacmod/**
Vehicle
Vehicle
<CAN>
<CAN>
ASといろいろあったよASといろいろあったよ
JapanTaxi
JapanTaxi
pacmod3
pacmod3
ゴルフカート!!ゴルフカート!!
YMC GolfCart
YMC GolfCart
top/velodyne_packets
top/velodyne_packets
velodyneのdriverです。VLP16, VLP32, VLS128, Livoxなどなど、たくさん種類があります。velodyneのdriverです。VLP16, VLP32, VLS128, Livoxなどなど、たくさん種類があります。
lidar_driver
lidar_driver
top/rectified/pointcloud
top/rectified/pointcloud
自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)
fix_distortion
fix_distortion
top/pointcloud_raw
top/pointcloud_raw
velodyneのpacketをpointcloud型に変更するvelodyneのpacketをpointcloud型に変更する
packets_to_
pointcloud
packets_to_...
top/self_cropped/pointcloud
top/self_cropped/pointcloud
自車の領域内にある点群を除去する。自車の領域内にある点群を除去する。
crop_box_
filter_self
crop_box_...
top/mirror_cropped/pointcloud
top/mirror_cropped/pointcloud
自車のサイドミラーの部分に当たった点群を除去する自車のサイドミラーの部分に当たった点群を除去する
crop_box_
filter_mirror
crop_box_...
点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)
ring_outlier_filter
ring_outlier_filter
lidar/*/velodyne_packets
lidar/*/velodyne_packets
lidar_drivers
(left,right,front_right, front_left,rear)
lidar_drivers...
*/rectified/pointcloud
*/rectified/pointcloud
自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)
fix_distortion
fix_distortion
*/pointcloud_raw
*/pointcloud_raw
velodyneのpacketをpointcloud型に変更するvelodyneのpacketをpointcloud型に変更する
packets_to_
pointcloud
packets_to_...
*/self_cropped/pointcloud
*/self_cropped/pointcloud
自車の領域内にある点群を除去する。自車の領域内にある点群を除去する。
crop_box_
filter_self
crop_box_...
*/mirror_cropped/pointcloud
*/mirror_cropped/pointcloud
自車のサイドミラーの部分に当たった点群を除去する自車のサイドミラーの部分に当たった点群を除去する
crop_box_
filter_mirror
crop_box_...
点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)
ring_outlier_filter
ring_outlier_filter
concatenated/pointcloud
concatenated/pointcloud
複数のLiDAR点群をまとめて1つにする複数のLiDAR点群をまとめて1つにする
concat_filter
concat_filter
MGRS座標系への変換MGRS座標系への変換
gnss_poser
gnss_poser
gnssのdriverですgnssのdriverです
gnss_driver
gnss_driver
imuのdriverですimuのdriverです
imu_driver
imu_driver
/sensing/imu/imu_raw
/sensing/imu/imu_raw
/sensing/lidar/pointcloud
/sensing/lidar/pointcloud
/sensing/lidar/
concatenated/pointcloud
/sensing/lidar/...
/sensing/gnss/
pose_with_covariance
/sensing/gnss/...
ヨーレートのバイアス除去とかをするノードヨーレートのバイアス除去とかをするノード
imu_corrector
imu_corrector
/sensing/imu/imu_data
/sensing/imu/imu_data
Sensing
Sensing
Localization
Localization
各々データを監視し、異常があればdiagで通知します各々データを監視し、異常があればdiagで通知します
system_monitors
system_monitors
System
System
autoware_process_
monitor
autoware_process_...
autoware_gpu_
monitor
autoware_gpu_...
autoware_mem_
monitor
autoware_mem_...
autoware_hdd_
monitor
autoware_hdd_...
autoware_net_
monitor
autoware_net_...
autoware_ntp_
monitor
autoware_ntp_...
/system/emergency/hazard_status
/system/emergency/hazard_status
system_error_
monitor
system_error_...
/system/emergency/control_cmd [AckermannControlCommand]
/system/emergency/gear_cmd [GearCommand]
/system/emergency/hazard_lights_cmd [HazardLightsCommand]
/system/emergency/emergency_state [EmergencyStateStamped]
/system/emergency/control_cmd [AckermannControlCommand]...
なにかしてますなにかしてます
emergency_handler
emergency_handler
/autoware/state
/autoware/state
/diagnostics
/diagnostics
なにかしてますなにかしてます
ad_service_
state_monitor
ad_service_...
/control/current_gate_mode
/vehicle/status/control_mode
/autoware/state
/control/c...
    /control/command/control_cmd
    /control/trajectory_follower/control_cmd
    /initialpose2d
    /localization/kinematic_state
    /map/pointcloud_map
    /map/vector_map
    /perception/object_recognition/objects
    /planning/mission_planning/route
    /planning/scenario_planning/trajectory
    /system/emergency/control_cmd
    /vehicle/status/control_mode
    /vehicle/status/steering_status
    /vehicle/status/velocity_status
/contr...
/autoware/engage
/autoware/engage
Text is not SVG - cannot display
\ No newline at end of file +
Planning
Planning
Control
Control
Perception
Perception
Vehicle
Vehicle
Sensing
Sensing
VehicleInterface
VehicleInterface
Localization
Localization
System
System
/diagnostics
/diagnostics
XX1
XX1
X2
X2
X1
X1
S1
S1
/vehicle/status/velocity_report
/vehicle/status/velocity_report
/autoware/engage [Engage]
/current_gate_mode [GateMode]
/autoware/engage [Engage]...
/planning/scenario_planning/max_velocity_candidates
[tier4_planning_msgs/msg/VelocityLimit]
/planning/scenario_planning/max_velocity_candidates...
HMI
HMI
HMI
HMI
HMI
HMI
/api/external/set/command/remote/control: [tier4_external_api_msgs/msg/ControlCommandStamped]
/api/external/set/command/remote/heartbeat: [tier4_external_api_msgs/msg/Heartbeat]
/api/external/set/command/remote/shift: [tier4_external_api_msgs/msg/GearShiftStamped]
/api/external/set/command/remote/turn_signal: [tier4_external_api_msgs/msg/TurnSignalStamped]
/api/external/set/command/remote/control: [tier4_external_api_msgs/msg/ControlCommandStamped]...
HMI
HMI
/vehicle/engage
/vehicle/engage
HMI
HMI
/diagnostics_err
/diagnostics_err
/initialpose
/initialpose
<pose_initializer
_srv>
<pose_initializer...
vehicle_velocity_converter/
twist_with_covariance
vehicle_velocity_converter/...
HMI
HMI
pointcloud_map_
loader
pointcloud_map_...
<differential_map_
loader_srv>
<differential_map_...
<partial_map_
loader_srv>
<partial_map_...
voxel_grid_downsample/pointcloud
voxel_grid_downsample/pointcloud
立体格子の点群密度が一様になるように点群を間引く。点群密度が正規化されるため、NDTのロバスト化に一役買っている。立体格子の点群密度が一様になるように点群を間引く。点群密度が正規化されるため、NDTのロバスト化に一役買っている。
voxel_grid_
downsample_filter
voxel_grid_...
pose_twist_fusion_filter/
pose_with_covariance_
no_yawbias
pose_twist_fusion_filter/...
Estimate ego vehicle's position, orientation, and velocity by EKF algorithm.Estimate ego vehicle's position, orientation, and velocity by EKF algorithm.
ekf_localizer
ekf_localizer
pose_estimator/
pose_with_covariance
pose_estimator/...
LiDARの点群とpointcloud_mapとのマッチングを行い、自己位置を計算するLiDARの点群とpointcloud_mapとのマッチングを行い、自己位置を計算する
ndt_scan_matcher
ndt_scan_matcher
downsample/pointcloud
downsample/pointcloud
点群数が一定以下になるようにランダムに間引く。基本的にはvoxel grid filterで丁度いいくらい点群数になるように調整してあり、点群数のlimiter的な立ち位置が強い 点群数が一定以下になるようにランダムに間引く。基本的にはvoxel grid filterで丁度いいくらい点群数になるように調整してあり、点群数のlimiter的な立ち位置が強い
random_
downsample_filter
random_...
/initialpose3d
/initialpose3d
GNSS / Userからラフな初期位置を受け取り、NDT+モンテカルロ法で正確な自己位置を推定して出力する。NDTとはserviceでやり取りする。GNSS / Userからラフな初期位置を受け取り、NDT+モンテカルロ法で正確な自己位置を推定して出力する。NDTとはserviceでやり取りする。
pose_initializer
pose_initializer
twist_estimator/twist_with_covariance
twist_estimator/twist_with_covariance
車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。
gyro_odometer
gyro_odometer
measurement_range/pointcloud
measurement_range/pointcloud
一定範囲内のLiDAR点群のみを切り取って出力する。ホントは遠くまで見たいけど、歪みの影響が大きくなったり、そもそも遠方の点群地図がなかったりするので、近くだけを見てる。一定範囲内のLiDAR点群のみを切り取って出力する。ホントは遠くまで見たいけど、歪みの影響が大きくなったり、そもそも遠方の点群地図がなかったりするので、近くだけを見てる。
crop_box_filter_
measurement_range
crop_box_filter_...
車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。車両速度とimuを統合する。いまは確か、ただ単にvx=vehicle, wz=imuだった気がする。
vehicle_velocity_converter
vehicle_velocity_converter
/tf
(map to base_link)
/tf...
/localization/kinematic_state
[nav_msgs/msg/Odometry]
/localization/kinematic_state...
Overwrite vx=0 and wz=0 when the vehicle wheel speed is zero to indicate the stop condition.Overwrite vx=0 and wz=0 when the vehicle wheel speed is zero to indicate the stop condition.
stop_filter
stop_filter
localization_error_monitor
localization_error_monitor
pose_twist_fusion_filter/
pose_with_covariance
pose_twist_fusion_filter/...
/diagnostics
/diagnostics
pose_twist_fusion_filter/
kinematic_state
pose_twist_fusion_filter/...
Map
Map
/map
/vector_map
/map...
lanelet2_map_loader
lanelet2_map_loader
/map/
map_projector_info
/map/...
map_projection_
loader
map_projection_...
Perception
Perception
/perception/object_recognition/objects
/perception/object_recognition/objects
traffic_light_states
traffic_light_states
/perception/traffic_light_recognition/
/perception/traffic_light_...
/perception/object_recognition/
/perception/object_recognition/
/perception/object_recognition/detection/
/perception/object_recognition/detectio...
clustering/clusters
clustering/clusters
euclidean_cluster
euclidean_cluster
pointcloud_map_filtered/pointcloud
pointcloud...

detection_by
_tracker/objects

detection_by...
centerpoint/validation/objects
centerpoint/validation/objects
DNNベースでLiDAR点群に物体のクラス情報を付与するDNNベースでLiDAR点群に物体のクラス情報を付与する
lidar_centerpoint
lidar_centerpoint
高精度地図情報を用いて、trackingされた動物体情報の移動経路予測を行う高精度地図情報を用いて、trackingされた動物体情報の移動経路予測を行う
map_based_
prediction
map_based_...
traffic_light_detection/rough/rois
traffic_light_detection/rough/rois
自己位置および高精度地図情報、自身の走行ルート情報から、信号機のおおよその位置を計算する自己位置および高精度地図情報、自身の走行ルート情報から、信号機のおおよその位置を計算する
traffic_light_map_
based_detector
traffic_light_map_...
detection/objects
detection/objects
画像に含まれる信号機の色を識別する画像に含まれる信号機の色を識別する
traffic_light_
classifier
traffic_light_...
画像処理を用いて信号機の正確な位置を計算する画像処理を用いて信号機の正確な位置を計算する
traffic_light_
ssd_fine_detector
traffic_light_...
route,
vector_map
route,...
クラス+位置+形状情報に対してtrackingを行う。(最近上流が速度情報も出せるようになってきたらしい)クラス+位置+形状情報に対してtrackingを行う。(最近上流が速度情報も出せるようになってきたらしい)
multi_object_
tracker
multi_object_...
detection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeするdetection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeする
object_association
_merger
object_association...
clustering結果の形状を推定するclustering結果の形状を推定する
shape_estimation
shape_estimation
/perception/occupancy_grid_map/
/perception/occupancy_grid_map/
/perception/obstacle_segmentation/
/perception/obstacle_segmentation/
no_ground/oneshot/pointcloud
no_ground/oneshot/pointcloud
range_cropped/pointcloud
range_cropped/pointcloud
アルゴリズムを使って点群から地面を除去します。アルゴリズムを使って点群から地面を除去します。
common_
ground_filter
common_...
occupancy_grid
occupancy_grid
/perception/occupancy_grid_map/map
/perception/occupancy_grid_map/map
全点群データ、地面除去済み点群、occupancy gridの情報を総合的に見て、障害物が存在するかどうかの確率をoccupancy gridマップの形式で計算します。全点群データ、地面除去済み点群、occupancy gridの情報を総合的に見て、障害物が存在するかどうかの確率をoccupancy gridマップの形式で計算します。
probabilistic_
occupancy_grid_map
probabilistic_...
occupancy gridで与えられる障害物存在確率マップの情報をもとに、障害物点群をフィルタリングします。occupancy gridで与えられる障害物存在確率マップの情報をもとに、障害物点群をフィルタリングします。
occupancy_grid_
map_based_outlier_filter
occupancy_grid_...
/perception/obstacle_segmentation/pointcloud
/perception/obstacle_segmentation/pointcloud
crop_box_filter
crop_box_filter
/concatenated/pointcloud
/concatena...
BBox内に存在するobstacle_segmentation後の点群数を用いて,false positiveを除くBBox内に存在するobstacle_segmentation後の点群数を用いて,false positiveを除く
obstacle_pointcloud _based_validator
obstacle_pointcloud _ba...
detection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeするdetection同士のassignmentを取り,confidenceが高い方を採用する.overlapしたunknown objectはmergeする
object_association
_merger
object_association...
DNNベースで画像に物体のクラス情報を付与するDNNベースで画像に物体のクラス情報を付与する
tensorrt_yolo
tensorrt_yolo
/perception/obstacle_segmentation/pointcloud
/perception/obstacle_segmentation/pointcloud
clustering結果に画像のdetection結果をprojectionしてlabelを付与するclustering結果に画像のdetection結果をprojectionしてlabelを付与する
roi_cluster_fusion
roi_cluster_fusion
/rois*
/rois*
clustering結果の形状を推定するclustering結果の形状を推定する
shape_estimation
shape_estimation
tracker内部のclusterをマージし,shape fittingしたbboxを出力するtracker内部のclusterをマージし,shape fittingしたbboxを出力する
detection_by_
tracker_node
detection_by_...
clustering/camera_lidar_fusion
/objects
clustering/camera_lidar_fusion...
/sensing/camera/
camera*/image_rect_color
/sensing/camera/...
vector mapの情報を用いて,unknown objectをfilterする.lane内のunknown objectのみを残す.vector mapの情報を用いて,unknown objectをfilterする.lane内のunknown objectのみを残す.
object_lanelet_filter
object_lanelet_filter
/map/vector_map
/map/vector_map
/map/vector_map
/map/vector_map
compare_map_filter
compare_map_filter
/perception/obstacle_segmentation/pointcloud
/perception/obstacle_segmentation/pointcloud
map
map
交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。
behavior_velocity_planner
behavior_velocity_planner
Planning
Planning
/planning/scenario_planning/trajectory [Trajectory]
/planning/scenario_planning/trajectory [Trajectory]
motion_planning/obstacle_avoidance_planner/trajectory [Trajectory]
motion_planning/obstacle_avoidance_planner/trajectory [Trajectory]
車両形状や走行可能領域を考慮して、経路の平滑化を行う。障害物も避けるが、long historyで最近は回避機能はoffになっています。車両形状や走行可能領域を考慮して、経路の平滑化を行う。障害物も避けるが、long historyで最近は回避機能はoffになっています。
obstacle_
avoidance_planner
obstacle_...
motion_planning/obstacle_velocity_limiter/trajectory [Trajectory]
motion_planning/obstacle_velocity_limiter/trajectory [Trajectory]
自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。
obstacle_velocity_limiter
obstacle_velocity_limit...
障害物停止・前車追従・近傍障害物減速の複数の機能を持ち、機能に応じて経路上に速度を埋め込む。 目標経路上に障害物点がある場合は停止、経路近くに障害物点群がある場合は減速、前方に車両が居る場合は前車追従。障害物停止・前車追従・近傍障害物減速の複数の機能を持ち、機能に応じて経路上に速度を埋め込む。 目標経路上に障害物点がある場合は停止、経路近くに障害物点群がある場合は減速、前方に車両が居る場合は前車追従。
obstacle_stop_
planner
obstacle_stop_...
障害物点群や地図の走行可能エリアの情報から、駐車プランナー用のコストマップを生成する。障害物点群や地図の走行可能エリアの情報から、駐車プランナー用のコストマップを生成する。
costmap_generator
costmap_generator
apiとautoware.iv内部から送られてくる制約付き減速指示を受け取り、整合性をとるapiとautoware.iv内部から送られてくる制約付き減速指示を受け取り、整合性をとる
external_velocity_
limit_selector
external_velocity_...
/planning/scenario_planning/max_velocity
[tier4_planning_msgs/msg/VelocityLimit]
/planning/scenario_planning/max_velocity...
/planning/scenario_planning/max_velocity_candidates
[tier4_planning_msgs/msg/VelocityLimit]
/planning/scenario_planning/max_velocity_candidates...
lane_driving/trajectory [Trajectory]
lane_driving/trajectory [Trajectory]
trajectory [Trajectory]
trajectory [Trajectory]
いまのrouteおよび地図情報から、LaneDriving / Parkingのシナリオを判断し、どちらの経路を使うかを切り替える。いまのrouteおよび地図情報から、LaneDriving / Parkingのシナリオを判断し、どちらの経路を使うかを切り替える。
scenario_selector
scenario_selector
/planning/scenario_planning/motion_velocity_smoother/trajectory [Trajectory]
/planning/scenario_planning/motion_velocity_smoother/trajectory [Trajectory]
経路に埋め込まれた最大速度を超過しないように、うまい具合で速度を平滑化する。カーブでの減速もここで行う。経路に埋め込まれた最大速度を超過しないように、うまい具合で速度を平滑化する。カーブでの減速もここで行う。
motion_velocity_
smoother
motion_velocity_...
<Lane Driving>
<Lane Driving>
停止禁止区域内で停止しないように、区域手前での進行・停止判断を行います。停止禁止区域内で停止しないように、区域手前での進行・停止判断を行います。
no_stopping_area
no_stopping_area
地図情報および動物体情報から、死角手前での減速を行います。地図情報および動物体情報から、死角手前での減速を行います。
occlusion_spot
occlusion_spot
地図情報および動物体情報から、交差点での停止/発進の判定を行います。地図情報および動物体情報から、交差点での停止/発進の判定を行います。
intersection
intersection
信号の色に応じて停止/発進の指示をします信号の色に応じて停止/発進の指示をします
traffic_light
traffic_light
横断歩道に人が居る、もしくは侵入しようとしてきている場合に、横断歩道手前で停止します。近くに人が居る場合は徐行します。横断歩道に人が居る、もしくは侵入しようとしてきている場合に、横断歩道手前で停止します。近くに人が居る場合は徐行します。
cross_walk
cross_walk
右左折時に車両後方の巻き込み確認を行い、衝突の危険がある場合は停止します。右左折時に車両後方の巻き込み確認を行い、衝突の危険がある場合は停止します。
blind_spot
blind_spot
規定エリア内の障害物点群がある場合に、対応する位置で停止します規定エリア内の障害物点群がある場合に、対応する位置で停止します
detection_area
detection_area
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
stop_line
stop_line
/planning/mission_planning/route
[autoware_auto_planning_msgs/HADMapRoute]
/planning/mission_planning/route...
高精度地図情報をもとに、自己位置からゴールまでのルートを計算します。高精度地図情報をもとに、自己位置からゴールまでのルートを計算します。
mission_planner
mission_planner
behavior_planning/path_with_lane_id
behavior_planning/path_with_lane_id
コストマップとゴールから、自車の走行ルートを計算する。いまはHA*を使ってる。コストマップとゴールから、自車の走行ルートを計算する。いまはHA*を使ってる。
freespace_planner
freespace_planner
costmap_generator/occupancy_grid
costmap_generator/occupancy_grid
goal
goal
behavior_planning/path
behavior_planning/path
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
no_drivable_lane
no_drivable_lane
<Parking>
<Parking>
parking/trajectory [Trajectory]
parking/trajectory [Trajectory]
自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。自車が停止しているときに、周囲の障害物を確認し、近くに障害物がいる場合は車両を発進させない。
surround_obstacle_checker
surround_obstacle_checker
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
run_out
run_out
信号の色に応じて停止/発進の指示をします信号の色に応じて停止/発進の指示をします
virtual_traffic_light
virtual_traffic_light
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
speed_bump
speed_bump
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
walkway
walkway
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
out_of_lane
out_of_lane
規定された停止線で一時停止を行います規定された停止線で一時停止を行います
merge_from_private
merge_from_private
交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。交通ルールをもとに、経路上の最大速度を決定します。各モジュールは、自身の計算した最大速度と上段の結果を比較し、小さい速度を経路上に書き込みます。
behavior_path_planner
behavior_path_planner
遠隔からの司令をもとに幅寄せを行います。遠隔からの司令をもとに幅寄せを行います。
side_shift
side_shift
ルート情報をもとに路肩への幅寄せ、路肩からの発進を行います。ルート情報をもとに路肩への幅寄せ、路肩からの発進を行います。
pull_over/out
pull_over/out
ルート情報や障害物情報をもとに、障害物回避を実行します。ルート情報や障害物情報をもとに、障害物回避を実行します。
obstacle_avoidance
obstacle_avoidance
ルート情報や障害物情報をもとに、レーンチェンジを実行します。ルート情報や障害物情報をもとに、レーンチェンジを実行します。
lane_change
lane_change
ルート情報をもとに走行経路を作成します。ルート情報をもとに走行経路を作成します。
lane_following
lane_following
Check the trajectory and stop publishing it if it is unsafe.Check the trajectory and stop publishing it if it is unsafe.
planning_validator
planning_validator
/planning/turn_indicators_cmd [TurnIndicatorsCommand]
/planning/hazard_lights_cmd [HazardLightsCommand]
/planning/turn_indicators_cmd [TurnIndicatorsCommand]...
path_smoother/path
path_smoother/path
車両形状や走行可能領域を考慮して、経路の平滑化を行う。障害物も避けるが、long historyで最近は回避機能はoffになっています。車両形状や走行可能領域を考慮して、経路の平滑化を行う。障害物も避けるが、long historyで最近は回避機能はoffになっています。
path_smoother
path_smoother
trajectory_follower
trajectory_follower
Control
Control
 /control/trajectory_follower/lateral/control_cmd
 [AckermannLateralControlCommand]

/control/trajectory_follower/lateral/control_cmd...
/control/trajectory_follower/longitudinal/control_cmd
[LongitudinalControlCommand]
/control/trajectory_follower/longitudinal/control_cmd...
/control/trajectory_follower/control_cmd
 [AckermannControlCommand]

/control/trajectory_follower/control_cmd...
経路追従のための目標ステア角とステア角速度を計算します。目標速度と加速度は経路に埋め込まれた値をそのまま出力します。経路追従のための目標ステア角とステア角速度を計算します。目標速度と加速度は経路に埋め込まれた値をそのまま出力します。
lateral_controller
lateral_controller
経路の目標速度に沿うための目標加速度を計算します。遅延補正や停止時のブレーキ抜き操作も行います。経路の目標速度に沿うための目標加速度を計算します。遅延補正や停止時のブレーキ抜き操作も行います。
longitudinal_controller
longitudinal_controller
指示モードに応じて、複数のコマンドの中から後段に渡すコマンドを選択します。指示モードに応じて、複数のコマンドの中から後段に渡すコマンドを選択します。
vehicle_cmd_gate
vehicle_cmd_gate
control/command/control_cmd
 [AckermannControlCommand]
control/command/control_cmd...
/control/shift_decider/gear_cmd
[GearCommand]
/control/shift_decider/gear_cmd...
制御出力に応じてギア変更のコマンドを出力します制御出力に応じてギア変更のコマンドを出力します
shift_decider
shift_decider
遠隔から来たコマンド(アクセル/ブレーキ)を目標速度/加速度の信号に変換します遠隔から来たコマンド(アクセル/ブレーキ)を目標速度/加速度の信号に変換します
external_cmd_
converter
external_cmd_...
/diagnostics
/diagnostics
MPCの出す予測経路と、laneletのレーン情報を用いて、車両がレーン外に出ようとしているかどうかを判定する。diag出力をerror_monitorに送り、逸脱判断字は停止する(option)MPCの出す予測経路と、laneletのレーン情報を用いて、車両がレーン外に出ようとしているかどうかを判定する。diag出力をerror_monitorに送り、逸脱判断字は停止する(option)
lane_departure_
checker
lane_departure_...
predicted_trajectory
predicted_trajectory
/external/selected/external_control_cmd
 [tier4_external_api_msgs/msg/ControlCommandStamped]
/external/selected/external_control_cmd...
external_cmd_
selector
external_cmd_...
/api/external/set/command/remote/control 
   [tier4_external_api_msgs/msg/ControlCommandStamped]
/api/external/set/command/remote/shift
   [tier4_external_api_msgs/msg/GearShiftStamped]
/api/external/set/command/remote/turn_signal
   [tier4_external_api_msgs/msg/TurnSignalStamped]
/api/external/set/command/remote/control...
z
z
/current_gate_mode [GateMode]
/gear_cmd [GearCommand]
/turn_indicators_cmd  [TurnIndicatorCommand]
/hazard_lights_cmd  [HazardLightsCommand]
/vehicle_emergency_cmd [VehicleEmergencyStamped]
/current_gate_mode [GateMode]...
ヤマハのGCとのインターフェースヤマハのGCとのインターフェース
g30_interface
g30_interface
速度・加速度の目標値を、車両特有の制御信号(アクセル/ブレーキ等)に変換します速度・加速度の目標値を、車両特有の制御信号(アクセル/ブレーキ等)に変換します
raw_vehicle_cmd_
converter
raw_vehicle_cmd_...
/vehicle/command/actuation_cmd
/vehicle/command/actuation_cmd
pacmodとのinterfaced。LexusとかJapanTaxiとか。pacmodとのinterfaced。LexusとかJapanTaxiとか。
pacmod_interface
pacmod_interface
VehicleInterface
VehicleInterface
/vehicle/status/velocity_status
/vehicle/status/steering_status
/vehicle/status/turn_indicators_status
/vehicle/status/gear_status
/vehicle/status/control_mode
/vehicle/status/actuation_status
/vehicle/s...
Autowareと車両のinterfaceです。データの変換が主なタスクです。Autowareと車両のinterfaceです。データの変換が主なタスクです。
    vehicle_interface
    vehicle_interface
accel_map_calibrator
accel_map_calibrator
/pacmod/**
/pacmod/**
Vehicle
Vehicle
<CAN>
<CAN>
ASといろいろあったよASといろいろあったよ
JapanTaxi
JapanTaxi
pacmod3
pacmod3
ゴルフカート!!ゴルフカート!!
YMC GolfCart
YMC GolfCart
top/velodyne_packets
top/velodyne_packets
velodyneのdriverです。VLP16, VLP32, VLS128, Livoxなどなど、たくさん種類があります。velodyneのdriverです。VLP16, VLP32, VLS128, Livoxなどなど、たくさん種類があります。
lidar_driver
lidar_driver
top/rectified/pointcloud
top/rectified/pointcloud
自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)
fix_distortion
fix_distortion
top/pointcloud_raw
top/pointcloud_raw
velodyneのpacketをpointcloud型に変更するvelodyneのpacketをpointcloud型に変更する
packets_to_
pointcloud
packets_to_...
top/self_cropped/pointcloud
top/self_cropped/pointcloud
自車の領域内にある点群を除去する。自車の領域内にある点群を除去する。
crop_box_
filter_self
crop_box_...
top/mirror_cropped/pointcloud
top/mirror_cropped/pointcloud
自車のサイドミラーの部分に当たった点群を除去する自車のサイドミラーの部分に当たった点群を除去する
crop_box_
filter_mirror
crop_box_...
点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)
ring_outlier_filter
ring_outlier_filter
lidar/*/velodyne_packets
lidar/*/velodyne_packets
lidar_drivers
(left,right,front_right, front_left,rear)
lidar_drivers...
*/rectified/pointcloud
*/rectified/pointcloud
自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)自車の回転によって生じたLiDAR点群の歪みを補正する。(リンクこれで合ってるっけ?)
fix_distortion
fix_distortion
*/pointcloud_raw
*/pointcloud_raw
velodyneのpacketをpointcloud型に変更するvelodyneのpacketをpointcloud型に変更する
packets_to_
pointcloud
packets_to_...
*/self_cropped/pointcloud
*/self_cropped/pointcloud
自車の領域内にある点群を除去する。自車の領域内にある点群を除去する。
crop_box_
filter_self
crop_box_...
*/mirror_cropped/pointcloud
*/mirror_cropped/pointcloud
自車のサイドミラーの部分に当たった点群を除去する自車のサイドミラーの部分に当たった点群を除去する
crop_box_
filter_mirror
crop_box_...
点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)点群のノイズ処理を行う (同一リングから得られたLiDARの点をクラスタリングし、点群数の少ないクラスタに属する点群はノイズとして除去される)
ring_outlier_filter
ring_outlier_filter
concatenated/pointcloud
concatenated/pointcloud
複数のLiDAR点群をまとめて1つにする複数のLiDAR点群をまとめて1つにする
concat_filter
concat_filter
MGRS座標系への変換MGRS座標系への変換
gnss_poser
gnss_poser
gnssのdriverですgnssのdriverです
gnss_driver
gnss_driver
imuのdriverですimuのdriverです
imu_driver
imu_driver
/sensing/imu/imu_raw
/sensing/imu/imu_raw
/sensing/lidar/pointcloud
/sensing/lidar/pointcloud
/sensing/lidar/
concatenated/pointcloud
/sensing/lidar/...
/sensing/gnss/
pose_with_covariance
/sensing/gnss/...
ヨーレートのバイアス除去とかをするノードヨーレートのバイアス除去とかをするノード
imu_corrector
imu_corrector
/sensing/imu/imu_data
/sensing/imu/imu_data
Sensing
Sensing
Localization
Localization
各々データを監視し、異常があればdiagで通知します各々データを監視し、異常があればdiagで通知します
system_monitors
system_monitors
System
System
autoware_process_
monitor
autoware_process_...
autoware_gpu_
monitor
autoware_gpu_...
autoware_mem_
monitor
autoware_mem_...
autoware_hdd_
monitor
autoware_hdd_...
autoware_net_
monitor
autoware_net_...
autoware_ntp_
monitor
autoware_ntp_...
/system/emergency/hazard_status
/system/emergency/hazard_status
system_error_
monitor
system_error_...
/system/emergency/control_cmd [AckermannControlCommand]
/system/emergency/gear_cmd [GearCommand]
/system/emergency/hazard_lights_cmd [HazardLightsCommand]
/system/emergency/emergency_state [EmergencyStateStamped]
/system/emergency/control_cmd [AckermannControlCommand]...
なにかしてますなにかしてます
emergency_handler
emergency_handler
/autoware/state
/autoware/state
/diagnostics
/diagnostics
なにかしてますなにかしてます
ad_service_
state_monitor
ad_service_...
/control/current_gate_mode
/vehicle/status/control_mode
/autoware/state
/control/c...
    /control/command/control_cmd
    /control/trajectory_follower/control_cmd
    /initialpose2d
    /localization/kinematic_state
    /map/pointcloud_map
    /map/vector_map
    /perception/object_recognition/objects
    /planning/mission_planning/route
    /planning/scenario_planning/trajectory
    /system/emergency/control_cmd
    /vehicle/status/control_mode
    /vehicle/status/steering_status
    /vehicle/status/velocity_status
/contr...
/autoware/engage
/autoware/engage
Text is not SVG - cannot display
\ No newline at end of file diff --git a/docs/design/autoware-interfaces/ad-api/features/cooperation.md b/docs/design/autoware-interfaces/ad-api/features/cooperation.md new file mode 100644 index 00000000000..b094e41340f --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/features/cooperation.md @@ -0,0 +1,94 @@ +# Cooperation + +## Related API + +- {{ link_ad_api('/api/planning/velocity_factors') }} +- {{ link_ad_api('/api/planning/steering_factors') }} +- {{ link_ad_api('/api/planning/cooperation/set_commands') }} +- {{ link_ad_api('/api/planning/cooperation/set_policies') }} +- {{ link_ad_api('/api/planning/cooperation/get_policies') }} + +## Description + +Request to cooperate (RTC) is a feature that enables a human operator to support the decision in autonomous driving mode. +Autoware usually drives the vehicle using its own decisions, but the operator may prefer to make their decisions in experiments and complex situations. + +The planning component manages each situation that requires decision as a scene. +Each scene has an ID that doesn't change until the scene is completed or canceled. +The operator can override the decision of the target scene using this ID. +In practice, the user interface application can hides the specification of the ID and provides an abstracted interface to the operator. + +For example, in the situation in the diagram below, vehicle is expected to make two lane changes and turning left at the intersection. +Therefore the planning component generates three scene instances for each required action, and each scene instance will wait for the decision to be made, in this case "changing or keeping lane" and "turning left or waiting at the intersection". +Here Autoware decides not to change lanes a second time due to the obstacle, so the vehicle will stop there. +However, operator could overwrite that decision through RTC function and force the lane change so that vehicle could reach to it's goal. +Using RTC, the operator can override these decisions to continue driving the vehicle to the goal. + +![cooperation-scenes](./cooperation/scenes.drawio.svg) + +## Architecture + +Modules that support RTC have the operator decision and cooperation policy in addition to the module decision as shown below. +These modules use the merged decision that is determined by these values when planning vehicle behavior. +See decisions section for details of these values. +The cooperation policy is used when there is no operator decision and has a default value set by the system settings. +If the module supports RTC, these information are available in [velocity factors or steering factors](./planning-factors.md) as [cooperation status](../types/autoware_adapi_v1_msgs/msg/CooperationStatus.md). + +![cooperation-architecture](./cooperation/architecture.drawio.svg) + +## Sequence + +This is an example sequence that overrides the scene decision to force a lane change. It is for the second scene in the diagram in the architecture section. +Here let's assume the cooperation policy is set to optional, see the decisions section described later for details. + +1. A planning module creates a scene instance with unique ID when approaching a place where a lane change is needed. +2. The scene instance generates the module decision from the current situation. In this case, the module decision is not to do a lane change due to the obstacle. +3. The scene instance generates the merged decision. At this point, there is no operator decision yet, so it is based on the module decision. +4. The scene instance plans the vehicle to keep the lane according to the merged decision. +5. The scene instance sends a cooperation status. +6. The operator receives the cooperation status. +7. The operator sends a cooperation command to override the module decision and to do a lane change. +8. The scene instance receives the cooperation command and update the operator decision. +9. The scene instance updates the module decision from the current situation. +10. The scene instance updates the merged decision. It is based on the operator decision received. +11. The scene instance plans the vehicle to change the lane according to the merged decision. + +## Decisions + +The merged decision is determined by the module decision, operator decision, and cooperation policy, each of which takes the value shown in the table below. + +| Status | Values | +| ------------------ | -------------------------------------- | +| merged decision | deactivate, activate | +| module decision | deactivate, activate | +| operator decision | deactivate, activate, autonomous, none | +| cooperation policy | required, optional | + +The meanings of these values are as follows. Note that the cooperation policy is common per module, so changing it will affect all scenes in the same module. + +| Value | Description | +| ---------- | ------------------------------------------------------------------------------------------ | +| deactivate | An operator/module decision to plan vehicle behavior with priority on safety. | +| activate | An operator/module decision to plan vehicle behavior with priority on driving. | +| autonomous | An operator decision that follows the module decision. | +| none | An initial value for operator decision, indicating that there is no operator decision yet. | +| required | A policy that requires the operator decision to continue driving. | +| optional | A policy that does not require the operator decision to continue driving. | + +The following flow is how the merged decision is determined. + +![cooperation-decisions](./cooperation/decisions.drawio.svg) + +## Examples + +This is an example of cooperation for lane change module. The behaviors by the combination of decisions are as follows. + +| Operator decision | Policy | Module decision | Description | +| ----------------- | -------- | --------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| deactivate | - | - | The operator instructs to keep lane regardless the module decision. So the vehicle keeps the lane by the operator decision. | +| activate | - | - | The operator instructs to change lane regardless the module decision. So the vehicle changes the lane by the operator decision. | +| autonomous | - | deactivate | The operator instructs to follow the module decision. So the vehicle keeps the lane by the module decision. | +| autonomous | - | activate | The operator instructs to follow the module decision. So the vehicle changes the lane by the module decision. | +| none | required | - | The required policy is used because no operator instruction. So the vehicle keeps the lane by the cooperation policy. | +| none | optional | deactivate | The optional policy is used because no operator instruction. So the vehicle keeps the lane by the module decision. | +| none | optional | activate | The optional policy is used because no operator instruction. So the vehicle change the lane by the module decision. | diff --git a/docs/design/autoware-interfaces/ad-api/features/cooperation/architecture.drawio.svg b/docs/design/autoware-interfaces/ad-api/features/cooperation/architecture.drawio.svg new file mode 100644 index 00000000000..77f18865950 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/features/cooperation/architecture.drawio.svg @@ -0,0 +1,260 @@ + + + + + + + + + + + + +
+
+
+ planning +
+ module +
+
+
+
+ + planning... + +
+
+ + + + + + +
+
+
+ merged decision +
+
+
+
+ + merged decision + +
+
+ + + + + + +
+
+
+ cooperation policy + +
+
+
+
+
+
+ + cooperation policy + +
+
+ + + + + + + + +
+
+
+ operator +
+
+
+
+ + operator + +
+
+ + + + +
+
+
+ cooperation status +
+
+
+
+ + cooperation stat... + +
+
+ + + + + + +
+
+
+ operator decision +
+
+
+
+ + operator decision + +
+
+ + + + + + +
+
+
+ module decision +
+
+
+
+ + module decision + +
+
+ + + + + + +
+
+
+ system +
+ settings +
+
+
+
+ + system... + +
+
+ + + + +
+
+
+ init +
+
+
+
+ + init + +
+
+ + + + +
+
+
+ set decision +
+
+
+
+ + set decision + +
+
+ + + + +
+
+
+ set policy +
+
+
+
+ + set policy + +
+
+ + + + +
+
+
+ for each scene class +
+
+
+
+ + for each scene class + +
+
+ + + + +
+
+
+ for each scene instance +
+
+
+
+ + for each scene instance + +
+
+
+ + + + + Text is not SVG - cannot display + + + +
\ No newline at end of file diff --git a/docs/design/autoware-interfaces/ad-api/features/cooperation/decisions.drawio.svg b/docs/design/autoware-interfaces/ad-api/features/cooperation/decisions.drawio.svg new file mode 100644 index 00000000000..14ce9fee5f4 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/features/cooperation/decisions.drawio.svg @@ -0,0 +1,337 @@ + + + + + + + + + + + + + + +
+
+
+ Check the operator decision. +
+
+
+
+ + Check the operator decision. + +
+
+ + + + + + + + + + + + + + + +
+
+
+ Check the cooperation policy. +
+
+
+
+ + Check the cooperation policy. + +
+
+ + + + +
+
+
+ [autonomous] +
+
+
+
+ + [autonomous] + +
+
+ + + + + + +
+
+
+ The merged decision is deactivate +
+ by the cooperation policy. +
+
+
+
+ + The merged decision is deactivate... + +
+
+ + + + +
+
+
+ [optional] +
+
+
+
+ + [optional] + +
+
+ + + + + + +
+
+
+ The merged decision is activate +
+ by the operator decision. +
+
+
+
+ + The merged decision is activate... + +
+
+ + + + + + + + + +
+
+
+ [none] +
+
+
+
+ + [none] + +
+
+ + + + +
+
+
+ [deactivate] +
+
+
+
+ + [deactivate] + +
+
+ + + + +
+
+
+ [activate] +
+
+
+
+ + [activate] + +
+
+ + + + +
+
+
+ + [required] + +
+
+
+
+ + [required] + +
+
+ + + + +
+
+
+ [deactivate] +
+
+
+
+ + [deactivate] + +
+
+ + + + + + +
+
+
+ Check the module decision. +
+
+
+
+ + Check the module decision. + +
+
+ + + + + + + + + + + +
+
+
+ The merged decision is activate +
+ by the module decision. +
+
+
+
+ + The merged decision is activate... + +
+
+ + + + + + +
+
+
+ The merged decision is deactivate +
+ by the module decision. +
+
+
+
+ + The merged decision is deactivate... + +
+
+ + + + + + +
+
+
+ The merged decision is deactivate +
+ by the operator decision. +
+
+
+
+ + The merged decision is deactivate... + +
+
+ + + + +
+
+
+ [activate] +
+
+
+
+ + [activate] + +
+
+
+ + + + + Text is not SVG - cannot display + + + +
\ No newline at end of file diff --git a/docs/design/autoware-interfaces/ad-api/features/cooperation/scenes.drawio.svg b/docs/design/autoware-interfaces/ad-api/features/cooperation/scenes.drawio.svg new file mode 100644 index 00000000000..9b813850ed5 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/features/cooperation/scenes.drawio.svg @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+
+ + obstacle + +
+
+ + + + + + + + + + +
+
+
+
+ Scene ID: 1234 +
+
+ Behavior: lane-change +
+
+ Decision: activate +
+
+
+
+
+ + Scene ID: 1234... + +
+
+ + + + + +
+
+
+ Scene ID + + : 5678 + +
+
+ Behavior: lane-change +
+
+ Decision: deactivate +
+
+
+
+
+ + Scene ID: 5678... + +
+
+ + + + + +
+
+
+ Scene ID + + : ABCD + +
+
+ Behavior: intersection +
+
+ Decision: activate +
+
+
+
+
+ + Scene ID: ABCD... + +
+
+
+ + + + + Text is not SVG - cannot display + + + +
\ No newline at end of file diff --git a/docs/design/autoware-interfaces/ad-api/features/planning-factors.md b/docs/design/autoware-interfaces/ad-api/features/planning-factors.md index eff614ae230..d595448687f 100644 --- a/docs/design/autoware-interfaces/ad-api/features/planning-factors.md +++ b/docs/design/autoware-interfaces/ad-api/features/planning-factors.md @@ -12,59 +12,58 @@ Applications can notify the vehicle behavior to the people around and visualize ## Velocity factors -The velocity factors is an array of information on the behavior that the vehicle stops (or slows down). -Each factor has a type shown below, pose in the base link, distance, status, and detailed data depending on its type. +The velocity factors is an array of information on the behavior that the vehicle stops or slows down. +Each factor has a behavior type which is described below. +Some behavior types have sequence and details as additional information. + +| Behavior | Description | +| --------------------------- | ----------------------------------------------------------------------------------- | +| surrounding-obstacle | There are obstacles immediately around the vehicle. | +| route-obstacle | There are obstacles along the route ahead. | +| intersection | There are obstacles in other lanes in the path. | +| crosswalk | There are obstacles on the crosswalk. | +| rear-check | There are obstacles behind that would be in a human driver's blind spot. | +| user-defined-attention-area | There are obstacles in the predefined attention area. | +| no-stopping-area | There is not enough space beyond the no stopping area. | +| stop-sign | A stop by a stop sign. | +| traffic-signal | A stop by a traffic signal. | +| v2x-gate-area | A stop by a gate area. It has enter and leave as sequences and v2x type as details. | +| merge | A stop before merging lanes. | +| sidewalk | A stop before crossing the sidewalk. | +| lane-change | A lane change. | +| avoidance | A path change to avoid an obstacle in the current lane. | +| emergency-operation | A stop by emergency instruction from the operator. | + +Each factor also provides status, poses in the base link frame, and distance from that pose. As the vehicle approaches the stop position, this factor appears with a status of APPROACHING. And when the vehicle reaches that position and stops, the status will be STOPPED. -The pose indicates the stop position or the base link if the stop position cannot be calculated. +The pose indicates the stop position, or the base link if the stop position cannot be calculated. ![velocity-factors](./planning-factors/velocity-factors.drawio.svg) -| Factor Type | Description | -| --------------------------- | ------------------------------------------------------------------------ | -| SURROUNDING_OBSTACLE | There are obstacles immediately around the vehicle. | -| ROUTE_OBSTACLE | There are obstacles along the route ahead. | -| INTERSECTION | There are obstacles in other lanes in the path. | -| CROSSWALK | There are obstacles on the crosswalk. | -| REAR_CHECK | There are obstacles behind that would be in a human driver's blind spot. | -| USER_DEFINED_DETECTION_AREA | There are obstacles in the predefined detection area. | -| NO_STOPPING_AREA | There is not enough space beyond the no stopping area. | -| STOP_SIGN | A stop by a stop sign. | -| TRAFFIC_SIGNAL | A stop by a traffic signal. | -| V2I_GATE_CONTROL_ENTER | A stop by a V2I gate entering. | -| V2I_GATE_CONTROL_LEAVE | A stop by a V2I gate leaving. | -| MERGE | A stop before merging lanes. | -| SIDEWALK | A stop before crossing the sidewalk. | -| LANE_CHANGE | A lane change. | -| AVOIDANCE | A path change to avoid an obstacle in the current lane. | -| EMERGENCY_OPERATION | A stop by emergency instruction from the operator. | - ## Steering factors The steering factors is an array of information on the maneuver that requires use of turn indicators, such as turning left or right. -Each factor has a type shown below, pose in the base link, distance, status, and detailed data depending on its type. +Each factor has a behavior type which is described below and steering direction. +Some behavior types have sequence and details as additional information. + +| Behavior | Description | +| ------------------- | --------------------------------------------------------------------------- | +| intersection | A turning left or right at an intersection. | +| lane-change | A lane change. | +| avoidance | A path change to avoid an obstacle. It has a sequence of change and return. | +| start-planner | T.B.D. | +| goal-planner | T.B.D. | +| emergency-operation | A path change by emergency instruction from the operator. | + +Each factor also provides status, poses in the base link frame, and distances from that poses. As the vehicle approaches the position to start steering, this factor appears with a status of APPROACHING. And when the vehicle reaches that position, the status will be TURNING. -The pose indicates the start position when APPROACHING and the end position when TURNING. +The poses indicate the start and end position of the section where the status is TURNING. ![steering-factors-1](./planning-factors/steering-factors-1.drawio.svg) In cases such as lane change and avoidance, the vehicle will start steering at any position in the range depending on the situation. -As the vehicle approaches the start position of the range, this factor appears with a status of APPROACHING. -And when the vehicle reaches that position, the status will be TRYING. -Then, when it is possible, the vehicle will start steering and the status will be TURNING. -The pose indicates the start of the range (A) when APPROACHING and the end of the range (B) when TRYING. -The position to end steering (C to D) for TURNING depends on the position to start steering. +For these types, the section where the status is TURNING will be updated dynamically and the poses will follow that. ![steering-factors-2](./planning-factors/steering-factors-2.drawio.svg) - -| Factor Type | Description | -| --------------------- | ------------------------------------------------------------------------ | -| INTERSECTION | A turning left or right at an intersection. | -| LANE_CHANGE | A lane change. | -| AVOIDANCE_PATH_CHANGE | A path change to avoid an obstacle in the current lane. | -| AVOIDANCE_PATH_RETURN | A path change to return to the original lane after avoiding an obstacle. | -| STATION | T.B.D. (bus stop) | -| PULL_OUT | T.B.D. | -| PULL_OVER | T.B.D. | -| EMERGENCY_OPERATION | A path change by emergency instruction from the operator. | diff --git a/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-1.drawio.svg b/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-1.drawio.svg index a47339dbf64..b5c1d2eb3f3 100644 --- a/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-1.drawio.svg +++ b/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-1.drawio.svg @@ -1,4 +1,4 @@ - + @@ -60,7 +60,7 @@ - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-2.drawio.svg b/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-2.drawio.svg index b2f15b2ff99..74511043061 100644 --- a/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-2.drawio.svg +++ b/docs/design/autoware-interfaces/ad-api/features/planning-factors/steering-factors-2.drawio.svg @@ -1,4 +1,4 @@ - + @@ -7,8 +7,7 @@ - - + @@ -31,25 +30,6 @@ - - - - - - -
-
-
- TRYING -
-
-
-
- - TRYING - -
-
@@ -58,11 +38,11 @@ - + -
+
TURNING @@ -70,85 +50,17 @@
- + TURNING - - - - -
-
-
- A -
-
-
-
- - A - -
-
- - - - -
-
-
- B -
-
-
-
- - B - -
-
- - - - -
-
-
- C -
-
-
-
- - C - -
-
- - - - -
-
-
- D -
-
-
-
- - D - -
-
- Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/docs/design/autoware-interfaces/ad-api/index.md b/docs/design/autoware-interfaces/ad-api/index.md index d530255f586..35d60d980fb 100644 --- a/docs/design/autoware-interfaces/ad-api/index.md +++ b/docs/design/autoware-interfaces/ad-api/index.md @@ -25,7 +25,7 @@ Service providers can combine these use cases to define user stories and check i - [Drive to the designated position](./use-cases/drive-designated-position.md) - [Get on and get off](./use-cases/get-on-off.md) - [Vehicle monitoring](./use-cases/vehicle-monitoring.md) -- [Request to cooperate](./use-cases/request-to-cooperate.md) +- [Vehicle operation](./use-cases/vehicle-operation.md) ## Features @@ -39,3 +39,4 @@ Service providers can combine these use cases to define user stories and check i - [Fail-safe](./features/fail-safe.md) - [Vehicle status](./features/vehicle-status.md) - [Vehicle doors](./features/vehicle-doors.md) +- [Cooperation](./features/cooperation.md) diff --git a/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/get_policies.md b/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/get_policies.md new file mode 100644 index 00000000000..6ffce00a8a1 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/get_policies.md @@ -0,0 +1,22 @@ +--- +title: /api/planning/cooperation/get_policies +status: not released +method: function call +type: + name: autoware_adapi_v1_msgs/srv/GetCooperationPolicies + res: + - name: status + text: response status + - name: policies.behavior + text: The type of the target behavior. + - name: policies.sequence + text: The type of the target sequence. + - name: policies.policy + text: The type of the cooporation policy. +--- + +{% extends 'design/autoware-interfaces/templates/autoware-interface.jinja2' %} +{% block description %} +Get the default decision that is used instead when the operator's decision is undecided. +For details, see the [cooperation](../../../../features/cooperation.md). +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/set_commands.md b/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/set_commands.md new file mode 100644 index 00000000000..bcca69658ed --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/set_commands.md @@ -0,0 +1,21 @@ +--- +title: /api/planning/cooperation/set_commands +status: not released +method: function call +type: + name: autoware_adapi_v1_msgs/srv/SetCooperationCommands + req: + - name: commands.uuid + text: The ID in the cooperation status. + - name: commands.cooperator + text: The operator's decision. + res: + - name: status + text: response status +--- + +{% extends 'design/autoware-interfaces/templates/autoware-interface.jinja2' %} +{% block description %} +Set the operator's decision for cooperation. +For details, see the [cooperation](../../../../features/cooperation.md). +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/set_policies.md b/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/set_policies.md new file mode 100644 index 00000000000..463bf6af96c --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/list/api/planning/cooperation/set_policies.md @@ -0,0 +1,23 @@ +--- +title: /api/planning/cooperation/set_policies +status: not released +method: function call +type: + name: autoware_adapi_v1_msgs/srv/SetCooperationPolicies + req: + - name: policies.behavior + text: The type of the target behavior. + - name: policies.sequence + text: The type of the target sequence. + - name: policies.policy + text: The type of the cooporation policy. + res: + - name: status + text: response status +--- + +{% extends 'design/autoware-interfaces/templates/autoware-interface.jinja2' %} +{% block description %} +Set the default decision that is used instead when the operator's decision is undecided. +For details, see the [cooperation](../../../../features/cooperation.md). +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/list/api/planning/steering_factors.md b/docs/design/autoware-interfaces/ad-api/list/api/planning/steering_factors.md index 55afd3720b0..b5d0970e88a 100644 --- a/docs/design/autoware-interfaces/ad-api/list/api/planning/steering_factors.md +++ b/docs/design/autoware-interfaces/ad-api/list/api/planning/steering_factors.md @@ -9,14 +9,18 @@ type: text: The base link pose related to the steering factor. - name: factors.distance text: The distance from the base link to the above pose. - - name: factors.type - text: The type of the steering factor. - name: factors.direction text: The direction of the steering factor. - name: factors.status text: The status of the steering factor. + - name: factors.behavior + text: The behavior type of the steering factor. + - name: factors.sequence + text: The sequence type of the steering factor. - name: factors.detail text: The additional information of the steering factor. + - name: factors.cooperation + text: The cooperation status if the module supports. --- {% extends 'design/autoware-interfaces/templates/autoware-interface.jinja2' %} diff --git a/docs/design/autoware-interfaces/ad-api/list/api/planning/velocity_factors.md b/docs/design/autoware-interfaces/ad-api/list/api/planning/velocity_factors.md index 8758e6016e5..8889cbb69cf 100644 --- a/docs/design/autoware-interfaces/ad-api/list/api/planning/velocity_factors.md +++ b/docs/design/autoware-interfaces/ad-api/list/api/planning/velocity_factors.md @@ -9,12 +9,16 @@ type: text: The base link pose related to the velocity factor. - name: factors.distance text: The distance from the base link to the above pose. - - name: factors.type - text: The type of the velocity factor. - name: factors.status text: The status of the velocity factor. + - name: factors.behavior + text: The behavior type of the velocity factor. + - name: factors.sequence + text: The sequence type of the velocity factor. - name: factors.detail text: The additional information of the velocity factor. + - name: factors.cooperation + text: The cooperation status if the module supports. --- {% extends 'design/autoware-interfaces/templates/autoware-interface.jinja2' %} diff --git a/docs/design/autoware-interfaces/ad-api/list/index.md b/docs/design/autoware-interfaces/ad-api/list/index.md index 9005e34f5ad..a95a73b670b 100644 --- a/docs/design/autoware-interfaces/ad-api/list/index.md +++ b/docs/design/autoware-interfaces/ad-api/list/index.md @@ -14,6 +14,9 @@ - [/api/operation_mode/enable_autoware_control](./api/operation_mode/enable_autoware_control.md) - [/api/operation_mode/state](./api/operation_mode/state.md) - [/api/perception/objects](./api/perception/objects.md) +- [/api/planning/cooperation/get_policies](./api/planning/cooperation/get_policies.md) +- [/api/planning/cooperation/set_commands](./api/planning/cooperation/set_commands.md) +- [/api/planning/cooperation/set_policies](./api/planning/cooperation/set_policies.md) - [/api/planning/steering_factors](./api/planning/steering_factors.md) - [/api/planning/velocity_factors](./api/planning/velocity_factors.md) - [/api/routing/clear_route](./api/routing/clear_route.md) diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationCommand.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationCommand.md new file mode 100644 index 00000000000..c088129687d --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationCommand.md @@ -0,0 +1,18 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/msg/CooperationCommand +used: + - autoware_adapi_v1_msgs/srv/SetCooperationCommands +uses: + - autoware_adapi_v1_msgs/msg/CooperationDecision +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +unique_identifier_msgs/UUID uuid +autoware_adapi_v1_msgs/CooperationDecision cooperator +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationDecision.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationDecision.md new file mode 100644 index 00000000000..1725dd81f79 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationDecision.md @@ -0,0 +1,22 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/msg/CooperationDecision +used: + - autoware_adapi_v1_msgs/msg/CooperationCommand + - autoware_adapi_v1_msgs/msg/CooperationStatus +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +uint8 UNKNOWN = 0 +uint8 DEACTIVATE = 1 +uint8 ACTIVATE = 2 +uint8 AUTONOMOUS = 3 +uint8 UNDECIDED = 4 + +uint8 decision +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationPolicy.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationPolicy.md new file mode 100644 index 00000000000..58bfbc9ae8d --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationPolicy.md @@ -0,0 +1,21 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/msg/CooperationPolicy +used: + - autoware_adapi_v1_msgs/srv/GetCooperationPolicies + - autoware_adapi_v1_msgs/srv/SetCooperationPolicies +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +uint8 OPTIONAL = 1 +uint8 REQUIRED = 2 + +string behavior +string sequence +uint8 policy +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationStatus.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationStatus.md new file mode 100644 index 00000000000..eee513f2c67 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/CooperationStatus.md @@ -0,0 +1,21 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/msg/CooperationStatus +used: + - autoware_adapi_v1_msgs/msg/SteeringFactor + - autoware_adapi_v1_msgs/msg/VelocityFactor +uses: + - autoware_adapi_v1_msgs/msg/CooperationDecision +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +unique_identifier_msgs/UUID uuid +autoware_adapi_v1_msgs/CooperationDecision autonomous +autoware_adapi_v1_msgs/CooperationDecision cooperator +bool cancellable +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/ResponseStatus.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/ResponseStatus.md index 9918fa9323a..7a2b8f31505 100644 --- a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/ResponseStatus.md +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/ResponseStatus.md @@ -5,9 +5,12 @@ used: - autoware_adapi_v1_msgs/srv/AcceptStart - autoware_adapi_v1_msgs/srv/ChangeOperationMode - autoware_adapi_v1_msgs/srv/ClearRoute + - autoware_adapi_v1_msgs/srv/GetCooperationPolicies - autoware_adapi_v1_msgs/srv/GetDoorLayout - autoware_adapi_v1_msgs/srv/GetVehicleDimensions - autoware_adapi_v1_msgs/srv/InitializeLocalization + - autoware_adapi_v1_msgs/srv/SetCooperationCommands + - autoware_adapi_v1_msgs/srv/SetCooperationPolicies - autoware_adapi_v1_msgs/srv/SetDoorCommand - autoware_adapi_v1_msgs/srv/SetRoute - autoware_adapi_v1_msgs/srv/SetRoutePoints diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/SteeringFactor.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/SteeringFactor.md index 1b35ddeb8a3..907033274ab 100644 --- a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/SteeringFactor.md +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/SteeringFactor.md @@ -3,6 +3,8 @@ title: autoware_adapi_v1_msgs/msg/SteeringFactor used: - autoware_adapi_v1_msgs/msg/SteeringFactorArray +uses: + - autoware_adapi_v1_msgs/msg/CooperationStatus --- {% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} @@ -12,18 +14,6 @@ used: # constants for common use uint16 UNKNOWN = 0 -# constants for type -uint16 INTERSECTION = 1 -uint16 LANE_CHANGE = 2 -uint16 AVOIDANCE_PATH_CHANGE = 3 -uint16 AVOIDANCE_PATH_RETURN = 4 -uint16 STATION = 5 -uint16 PULL_OUT = 6 # Deprecated. Use START_PLANNER. -uint16 START_PLANNER = 6 -uint16 PULL_OVER = 7 # Deprecated. Use GOAL_PLANNER. -uint16 GOAL_PLANNER = 7 -uint16 EMERGENCY_OPERATION = 8 - # constants for direction uint16 LEFT = 1 uint16 RIGHT = 2 @@ -31,16 +21,37 @@ uint16 STRAIGHT = 3 # constants for status uint16 APPROACHING = 1 -uint16 TRYING = 2 uint16 TURNING = 3 # variables geometry_msgs/Pose[2] pose float32[2] distance -uint16 type uint16 direction uint16 status +string behavior +string sequence string detail +autoware_adapi_v1_msgs/CooperationStatus[<=1] cooperation + + + +# deprecated constants for type +uint16 INTERSECTION = 1 +uint16 LANE_CHANGE = 2 +uint16 AVOIDANCE_PATH_CHANGE = 3 +uint16 AVOIDANCE_PATH_RETURN = 4 +uint16 STATION = 5 +uint16 PULL_OUT = 6 # Deprecated. Use START_PLANNER. +uint16 START_PLANNER = 6 +uint16 PULL_OVER = 7 # Deprecated. Use GOAL_PLANNER. +uint16 GOAL_PLANNER = 7 +uint16 EMERGENCY_OPERATION = 8 + +# deprecated constants for status +uint16 TRYING = 2 + +# deprecated variables +uint16 type ``` {% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/VelocityFactor.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/VelocityFactor.md index 7cb4125992f..8749fcd7b0f 100644 --- a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/VelocityFactor.md +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/msg/VelocityFactor.md @@ -3,6 +3,8 @@ title: autoware_adapi_v1_msgs/msg/VelocityFactor used: - autoware_adapi_v1_msgs/msg/VelocityFactorArray +uses: + - autoware_adapi_v1_msgs/msg/CooperationStatus --- {% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} @@ -12,7 +14,22 @@ used: # constants for common use uint16 UNKNOWN = 0 -# constants for type +# constants for status +uint16 APPROACHING = 1 +uint16 STOPPED = 2 + +# variables +geometry_msgs/Pose pose +float32 distance +uint16 status +string behavior +string sequence +string detail +autoware_adapi_v1_msgs/CooperationStatus[<=1] cooperation + + + +# deprecated constants for type uint16 SURROUNDING_OBSTACLE = 1 uint16 ROUTE_OBSTACLE = 2 uint16 INTERSECTION = 3 @@ -31,16 +48,8 @@ uint16 AVOIDANCE = 15 uint16 EMERGENCY_STOP_OPERATION = 16 uint16 NO_DRIVABLE_LANE = 17 -# constants for status -uint16 APPROACHING = 1 -uint16 STOPPED = 2 - -# variables -geometry_msgs/Pose pose -float32 distance +# deprecated variables uint16 type -uint16 status -string detail ``` {% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/GetCooperationPolicies.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/GetCooperationPolicies.md new file mode 100644 index 00000000000..e6cc04266b2 --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/GetCooperationPolicies.md @@ -0,0 +1,18 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/srv/GetCooperationPolicies +uses: + - autoware_adapi_v1_msgs/msg/CooperationPolicy + - autoware_adapi_v1_msgs/msg/ResponseStatus +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +--- +autoware_adapi_v1_msgs/ResponseStatus status +autoware_adapi_v1_msgs/CooperationPolicy[] policies +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/SetCooperationCommands.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/SetCooperationCommands.md new file mode 100644 index 00000000000..be9e777245a --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/SetCooperationCommands.md @@ -0,0 +1,18 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/srv/SetCooperationCommands +uses: + - autoware_adapi_v1_msgs/msg/CooperationCommand + - autoware_adapi_v1_msgs/msg/ResponseStatus +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +autoware_adapi_v1_msgs/CooperationCommand[] commands +--- +autoware_adapi_v1_msgs/ResponseStatus status +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/SetCooperationPolicies.md b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/SetCooperationPolicies.md new file mode 100644 index 00000000000..b25068edd9a --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/types/autoware_adapi_v1_msgs/srv/SetCooperationPolicies.md @@ -0,0 +1,18 @@ +--- +# This file is generated by tools/autoware-interfaces/generate.py +title: autoware_adapi_v1_msgs/srv/SetCooperationPolicies +uses: + - autoware_adapi_v1_msgs/msg/CooperationPolicy + - autoware_adapi_v1_msgs/msg/ResponseStatus +--- + +{% extends 'design/autoware-interfaces/templates/autoware-data-type.jinja2' %} +{% block definition %} + +```txt +autoware_adapi_v1_msgs/CooperationPolicy[] policies +--- +autoware_adapi_v1_msgs/ResponseStatus status +``` + +{% endblock %} diff --git a/docs/design/autoware-interfaces/ad-api/types/index.md b/docs/design/autoware-interfaces/ad-api/types/index.md index fc2a043a3ac..c620381a216 100644 --- a/docs/design/autoware-interfaces/ad-api/types/index.md +++ b/docs/design/autoware-interfaces/ad-api/types/index.md @@ -1,5 +1,9 @@ # Types of Autoware AD API +- [autoware_adapi_v1_msgs/msg/CooperationCommand](./autoware_adapi_v1_msgs/msg/CooperationCommand.md) +- [autoware_adapi_v1_msgs/msg/CooperationDecision](./autoware_adapi_v1_msgs/msg/CooperationDecision.md) +- [autoware_adapi_v1_msgs/msg/CooperationPolicy](./autoware_adapi_v1_msgs/msg/CooperationPolicy.md) +- [autoware_adapi_v1_msgs/msg/CooperationStatus](./autoware_adapi_v1_msgs/msg/CooperationStatus.md) - [autoware_adapi_v1_msgs/msg/DoorCommand](./autoware_adapi_v1_msgs/msg/DoorCommand.md) - [autoware_adapi_v1_msgs/msg/DoorLayout](./autoware_adapi_v1_msgs/msg/DoorLayout.md) - [autoware_adapi_v1_msgs/msg/DoorStatus](./autoware_adapi_v1_msgs/msg/DoorStatus.md) @@ -33,9 +37,12 @@ - [autoware_adapi_v1_msgs/srv/AcceptStart](./autoware_adapi_v1_msgs/srv/AcceptStart.md) - [autoware_adapi_v1_msgs/srv/ChangeOperationMode](./autoware_adapi_v1_msgs/srv/ChangeOperationMode.md) - [autoware_adapi_v1_msgs/srv/ClearRoute](./autoware_adapi_v1_msgs/srv/ClearRoute.md) +- [autoware_adapi_v1_msgs/srv/GetCooperationPolicies](./autoware_adapi_v1_msgs/srv/GetCooperationPolicies.md) - [autoware_adapi_v1_msgs/srv/GetDoorLayout](./autoware_adapi_v1_msgs/srv/GetDoorLayout.md) - [autoware_adapi_v1_msgs/srv/GetVehicleDimensions](./autoware_adapi_v1_msgs/srv/GetVehicleDimensions.md) - [autoware_adapi_v1_msgs/srv/InitializeLocalization](./autoware_adapi_v1_msgs/srv/InitializeLocalization.md) +- [autoware_adapi_v1_msgs/srv/SetCooperationCommands](./autoware_adapi_v1_msgs/srv/SetCooperationCommands.md) +- [autoware_adapi_v1_msgs/srv/SetCooperationPolicies](./autoware_adapi_v1_msgs/srv/SetCooperationPolicies.md) - [autoware_adapi_v1_msgs/srv/SetDoorCommand](./autoware_adapi_v1_msgs/srv/SetDoorCommand.md) - [autoware_adapi_v1_msgs/srv/SetRoute](./autoware_adapi_v1_msgs/srv/SetRoute.md) - [autoware_adapi_v1_msgs/srv/SetRoutePoints](./autoware_adapi_v1_msgs/srv/SetRoutePoints.md) diff --git a/docs/design/autoware-interfaces/ad-api/use-cases/request-to-cooperate.md b/docs/design/autoware-interfaces/ad-api/use-cases/request-to-cooperate.md deleted file mode 100644 index 7b0dc2385d3..00000000000 --- a/docs/design/autoware-interfaces/ad-api/use-cases/request-to-cooperate.md +++ /dev/null @@ -1,7 +0,0 @@ -# Request to cooperate - -!!! warning - - Under Construction - -AD API supports operator cooperation for vehicle decisions. diff --git a/docs/design/autoware-interfaces/ad-api/use-cases/vehicle-operation.md b/docs/design/autoware-interfaces/ad-api/use-cases/vehicle-operation.md new file mode 100644 index 00000000000..4cfb7b2639e --- /dev/null +++ b/docs/design/autoware-interfaces/ad-api/use-cases/vehicle-operation.md @@ -0,0 +1,14 @@ +# Vehicle operation + +## Request to intervene + +Request to intervene (RTI) is a feature that requires the operator to switch to manual driving mode. It is also called Take Over Request (TOR). +Interfaces for RTI are currently being discussed. For now assume that manual driving is requested if the MRM state is not NORMAL. +See [fail-safe](../features/fail-safe.md) for details. + +## Request to cooperate + +Request to cooperate (RTC) is a feature that the operator supports the decision in autonomous driving mode. +Autoware usually drives the vehicle using its own decisions, but the operator may prefer to make their own decisions in complex situations. +Since RTC only overrides the decision and does not need to change operation mode, the vehicle can continue autonomous driving, unlike RTC. +See [cooperation](../features/cooperation.md) for details. diff --git a/docs/design/autoware-interfaces/components/perception-interface.md b/docs/design/autoware-interfaces/components/perception-interface.md index 00e13fbc2e5..a618241ebd7 100644 --- a/docs/design/autoware-interfaces/components/perception-interface.md +++ b/docs/design/autoware-interfaces/components/perception-interface.md @@ -99,7 +99,7 @@ traffic signals recognized by object detection model. - [autoware_perception_msgs::msg::TrafficSignalArray](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) - [autoware_perception_msgs::msg::TrafficSignal](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignal.msg) signals - [autoware_perception_msgs::msg::TrafficSignalElement](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalElement.msg) elements - - unint8 UNKNOWN = 0 + - uint8 UNKNOWN = 0 - uint8 Red = 1 - uint8 AMBER = 2 - uint8 WHITE = 4 diff --git a/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/index.md b/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/index.md index 1c4a8a5bf78..5d42f5e0a5b 100644 --- a/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/index.md +++ b/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/index.md @@ -16,7 +16,7 @@ Most of these algorithms already have a built-in loop-closure and pose graph opt ## List of Third Party SLAM Implementations - +

diff --git a/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/iscloam/index.md b/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/iscloam/index.md index 6274ebf87ab..6f79c40d248 100644 --- a/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/iscloam/index.md +++ b/docs/how-to-guides/integrating-autoware/creating-maps/open-source-slam/iscloam/index.md @@ -1,5 +1,3 @@ - - # ISCLOAM ## What is ISCLOAM? diff --git a/yaml/autoware-interfaces.yaml b/yaml/autoware-interfaces.yaml index a86ed502e4d..7ee9a89be63 100644 --- a/yaml/autoware-interfaces.yaml +++ b/yaml/autoware-interfaces.yaml @@ -1,4 +1,22 @@ types: + autoware_adapi_v1_msgs/msg/CooperationCommand: + msg: + cooperator: autoware_adapi_v1_msgs/msg/CooperationDecision + uuid: unique_identifier_msgs/msg/UUID + autoware_adapi_v1_msgs/msg/CooperationDecision: + msg: + decision: uint8 + autoware_adapi_v1_msgs/msg/CooperationPolicy: + msg: + behavior: string + policy: uint8 + sequence: string + autoware_adapi_v1_msgs/msg/CooperationStatus: + msg: + autonomous: autoware_adapi_v1_msgs/msg/CooperationDecision + cancellable: bool + cooperator: autoware_adapi_v1_msgs/msg/CooperationDecision + uuid: unique_identifier_msgs/msg/UUID autoware_adapi_v1_msgs/msg/DoorCommand: msg: command: uint8 @@ -100,10 +118,13 @@ types: state: uint16 autoware_adapi_v1_msgs/msg/SteeringFactor: msg: + behavior: string + cooperation: autoware_adapi_v1_msgs/msg/CooperationStatus[<=1] detail: string direction: uint16 distance: float32[2] pose: geometry_msgs/msg/Pose[2] + sequence: string status: uint16 type: uint16 autoware_adapi_v1_msgs/msg/SteeringFactorArray: @@ -141,9 +162,12 @@ types: turn_indicators: autoware_adapi_v1_msgs/msg/TurnIndicators autoware_adapi_v1_msgs/msg/VelocityFactor: msg: + behavior: string + cooperation: autoware_adapi_v1_msgs/msg/CooperationStatus[<=1] detail: string distance: float32 pose: geometry_msgs/msg/Pose + sequence: string status: uint16 type: uint16 autoware_adapi_v1_msgs/msg/VelocityFactorArray: @@ -159,6 +183,10 @@ types: autoware_adapi_v1_msgs/srv/ClearRoute: res: status: autoware_adapi_v1_msgs/msg/ResponseStatus + autoware_adapi_v1_msgs/srv/GetCooperationPolicies: + res: + policies: autoware_adapi_v1_msgs/msg/CooperationPolicy[] + status: autoware_adapi_v1_msgs/msg/ResponseStatus autoware_adapi_v1_msgs/srv/GetDoorLayout: res: doors: autoware_adapi_v1_msgs/msg/DoorLayout[] @@ -172,6 +200,16 @@ types: pose: geometry_msgs/msg/PoseWithCovarianceStamped[<=1] res: status: autoware_adapi_v1_msgs/msg/ResponseStatus + autoware_adapi_v1_msgs/srv/SetCooperationCommands: + req: + commands: autoware_adapi_v1_msgs/msg/CooperationCommand[] + res: + status: autoware_adapi_v1_msgs/msg/ResponseStatus + autoware_adapi_v1_msgs/srv/SetCooperationPolicies: + req: + policies: autoware_adapi_v1_msgs/msg/CooperationPolicy[] + res: + status: autoware_adapi_v1_msgs/msg/ResponseStatus autoware_adapi_v1_msgs/srv/SetDoorCommand: req: doors: autoware_adapi_v1_msgs/msg/DoorCommand[]