diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
index b77573e09..db998fae0 100644
--- a/.github/workflows/build-and-test-differential.yaml
+++ b/.github/workflows/build-and-test-differential.yaml
@@ -6,7 +6,20 @@ on:
jobs:
build-and-test-differential:
runs-on: ubuntu-latest
- container: ghcr.io/autowarefoundation/autoware-universe:latest
+ container: ${{ matrix.container }}
+ strategy:
+ fail-fast: false
+ matrix:
+ rosdistro:
+ - galactic
+ - humble
+ include:
+ - rosdistro: galactic
+ container: ros:galactic
+ build-depends-repos: build_depends.repos
+ - rosdistro: humble
+ container: ros:humble
+ build-depends-repos: build_depends.repos
steps:
- name: Cancel previous runs
uses: styfle/cancel-workflow-action@0.9.1
@@ -27,18 +40,18 @@ jobs:
if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
uses: autowarefoundation/autoware-github-actions/colcon-build@v1
with:
- rosdistro: galactic
+ rosdistro: ${{ matrix.rosdistro }}
target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
- build-depends-repos: build_depends.repos
+ build-depends-repos: ${{ matrix.build-depends-repos }}
- name: Test
id: test
if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
uses: autowarefoundation/autoware-github-actions/colcon-test@v1
with:
- rosdistro: galactic
+ rosdistro: ${{ matrix.rosdistro }}
target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
- build-depends-repos: build_depends.repos
+ build-depends-repos: ${{ matrix.build-depends-repos }}
- name: Upload coverage to CodeCov
if: ${{ steps.test.outputs.coverage-report-files != '' }}
diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml
index f0760d99a..161e3ba22 100644
--- a/.github/workflows/build-and-test.yaml
+++ b/.github/workflows/build-and-test.yaml
@@ -10,7 +10,20 @@ jobs:
build-and-test:
if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
runs-on: ubuntu-latest
- container: ghcr.io/autowarefoundation/autoware-universe:latest
+ container: ${{ matrix.container }}
+ strategy:
+ fail-fast: false
+ matrix:
+ rosdistro:
+ - galactic
+ - humble
+ include:
+ - rosdistro: galactic
+ container: ros:galactic
+ build-depends-repos: build_depends.repos
+ - rosdistro: humble
+ container: ros:humble
+ build-depends-repos: build_depends.repos
steps:
- name: Check out repository
uses: actions/checkout@v3
@@ -26,18 +39,18 @@ jobs:
if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
uses: autowarefoundation/autoware-github-actions/colcon-build@v1
with:
- rosdistro: galactic
+ rosdistro: ${{ matrix.rosdistro }}
target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
- build-depends-repos: build_depends.repos
+ build-depends-repos: ${{ matrix.build-depends-repos }}
- name: Test
if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
id: test
uses: autowarefoundation/autoware-github-actions/colcon-test@v1
with:
- rosdistro: galactic
+ rosdistro: ${{ matrix.rosdistro }}
target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
- build-depends-repos: build_depends.repos
+ build-depends-repos: ${{ matrix.build-depends-repos }}
- name: Upload coverage to CodeCov
if: ${{ steps.test.outputs.coverage-report-files != '' }}
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index 02d6c3363..2b7bf1861 100644
--- a/autoware_launch/launch/autoware.launch.xml
+++ b/autoware_launch/launch/autoware.launch.xml
@@ -78,7 +78,10 @@
-
+
+
+
+
diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
index 7fc575152..1cc48dbc8 100644
--- a/autoware_launch/launch/logging_simulator.launch.xml
+++ b/autoware_launch/launch/logging_simulator.launch.xml
@@ -94,7 +94,10 @@
-
+
+
+
+
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index 690e038e7..e36a04c7f 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -42,8 +42,7 @@ Visualization Manager:
Show Arrows: true
Show Axes: true
Show Names: true
- Tree:
- {}
+ Tree: {}
Update Interval: 0
Value: false
- Alpha: 0.5
@@ -68,12 +67,9 @@ Visualization Manager:
Displays:
- Class: rviz_plugins/SteeringAngle
Enabled: true
- Left: 128
- Length: 256
Name: SteeringAngle
Scale: 17
Text Color: 25; 255; 240
- Top: 128
Topic:
Depth: 5
Durability Policy: Volatile
@@ -84,12 +80,9 @@ Visualization Manager:
Value height offset: 0
- Class: rviz_plugins/ConsoleMeter
Enabled: true
- Left: 512
- Length: 256
Name: ConsoleMeter
Scale: 3
Text Color: 25; 255; 240
- Top: 128
Topic:
Depth: 5
Durability Policy: Volatile
@@ -303,11 +296,8 @@ Visualization Manager:
Wave Velocity: 40
- Class: rviz_plugins/MaxVelocity
Enabled: true
- Left: 595
- Length: 96
Name: MaxVelocity
Text Color: 255; 255; 255
- Top: 280
Topic:
Depth: 5
Durability Policy: Volatile
@@ -317,10 +307,7 @@ Visualization Manager:
Value: true
- Class: rviz_plugins/TurnSignal
Enabled: true
- Height: 256
- Left: 196
Name: TurnSignal
- Top: 350
Topic:
Depth: 5
Durability Policy: Volatile
@@ -328,7 +315,6 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /vehicle/status/turn_indicators_status
Value: true
- Width: 512
Enabled: true
Name: Vehicle
Enabled: true
@@ -650,8 +636,7 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MonteCarloInitialPose
- Namespaces:
- {}
+ Namespaces: {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -724,207 +709,174 @@ Visualization Manager:
Name: Segmentation
- Class: rviz_common/Group
Displays:
- - Class: rviz_common/Group
- Displays:
- - BUS:
- Alpha: 0.999
- Color: 30; 144; 255
- CAR:
- Alpha: 0.999
- Color: 30; 144; 255
- CYCLIST:
- Alpha: 0.999
- Color: 119; 11; 32
- Class: autoware_auto_perception_rviz_plugin/DetectedObjects
- Display 3d polygon: true
- Display Label: true
- Display PoseWithCovariance: true
- Display Predicted Path Confidence: true
- Display Predicted Paths: true
- Display Twist: true
- Display UUID: true
- Display Velocity: true
- Enabled: true
- MOTORCYCLE:
- Alpha: 0.999
- Color: 119; 11; 32
- Name: DetectedObjects
- Namespaces:
- {}
- PEDESTRIAN:
- Alpha: 0.999
- Color: 255; 192; 203
- TRAILER:
- Alpha: 0.999
- Color: 30; 144; 255
- TRUCK:
- Alpha: 0.999
- Color: 30; 144; 255
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /perception/object_recognition/detection/objects
- UNKNOWN:
- Alpha: 0.999
- Color: 255; 255; 255
- Value: true
- Enabled: false
- Name: Detection
- - Class: rviz_common/Group
- Displays:
- - BUS:
- Alpha: 0.999
- Color: 30; 144; 255
- CAR:
- Alpha: 0.999
- Color: 30; 144; 255
- CYCLIST:
- Alpha: 0.999
- Color: 119; 11; 32
- Class: autoware_auto_perception_rviz_plugin/TrackedObjects
- Display 3d polygon: true
- Display Label: true
- Display PoseWithCovariance: true
- Display Predicted Path Confidence: true
- Display Predicted Paths: true
- Display Twist: true
- Display UUID: true
- Display Velocity: true
- Enabled: true
- MOTORCYCLE:
- Alpha: 0.999
- Color: 119; 11; 32
- Name: TrackedObjects
- Namespaces:
- {}
- PEDESTRIAN:
- Alpha: 0.999
- Color: 255; 192; 203
- TRAILER:
- Alpha: 0.999
- Color: 30; 144; 255
- TRUCK:
- Alpha: 0.999
- Color: 30; 144; 255
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /perception/object_recognition/tracking/objects
- UNKNOWN:
- Alpha: 0.999
- Color: 255; 255; 255
- Value: true
- Enabled: false
- Name: Tracking
- - Class: rviz_common/Group
- Displays:
- - BUS:
- Alpha: 0.999
- Color: 30; 144; 255
- CAR:
- Alpha: 0.999
- Color: 30; 144; 255
- CYCLIST:
- Alpha: 0.999
- Color: 119; 11; 32
- Class: autoware_auto_perception_rviz_plugin/PredictedObjects
- Display 3d polygon: true
- Display Label: true
- Display PoseWithCovariance: true
- Display Predicted Path Confidence: true
- Display Predicted Paths: true
- Display Twist: true
- Display UUID: true
- Display Velocity: true
- Enabled: true
- MOTORCYCLE:
- Alpha: 0.999
- Color: 119; 11; 32
- Name: PredictedObjects
- Namespaces:
- {}
- PEDESTRIAN:
- Alpha: 0.999
- Color: 255; 192; 203
- TRAILER:
- Alpha: 0.999
- Color: 30; 144; 255
- TRUCK:
- Alpha: 0.999
- Color: 30; 144; 255
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /perception/object_recognition/objects
- UNKNOWN:
- Alpha: 0.999
- Color: 255; 255; 255
- Value: true
+ - BUS:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ CAR:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ CYCLIST:
+ Alpha: 0.999
+ Color: 119; 11; 32
+ Class: autoware_auto_perception_rviz_plugin/DetectedObjects
+ Display 3d polygon: true
+ Display Label: true
+ Display PoseWithCovariance: true
+ Display Predicted Path Confidence: true
+ Display Predicted Paths: true
+ Display Twist: true
+ Display UUID: true
+ Display Velocity: true
Enabled: true
- Name: Prediction
- Enabled: true
- Name: ObjectRecognition
+ MOTORCYCLE:
+ Alpha: 0.999
+ Color: 119; 11; 32
+ Name: DetectedObjects
+ Namespaces: {}
+ PEDESTRIAN:
+ Alpha: 0.999
+ Color: 255; 192; 203
+ TRAILER:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ TRUCK:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /perception/object_recognition/detection/objects
+ UNKNOWN:
+ Alpha: 0.999
+ Color: 255; 255; 255
+ Value: true
+ Enabled: false
+ Name: Detection
- Class: rviz_common/Group
Displays:
- - Class: rviz_default_plugins/Image
+ - BUS:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ CAR:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ CYCLIST:
+ Alpha: 0.999
+ Color: 119; 11; 32
+ Class: autoware_auto_perception_rviz_plugin/TrackedObjects
+ Display 3d polygon: true
+ Display Label: true
+ Display PoseWithCovariance: true
+ Display Predicted Path Confidence: true
+ Display Predicted Paths: true
+ Display Twist: true
+ Display UUID: true
+ Display Velocity: true
Enabled: true
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: RecognitionResultOnImage
- Normalize Range: true
+ MOTORCYCLE:
+ Alpha: 0.999
+ Color: 119; 11; 32
+ Name: TrackedObjects
+ Namespaces: {}
+ PEDESTRIAN:
+ Alpha: 0.999
+ Color: 255; 192; 203
+ TRAILER:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ TRUCK:
+ Alpha: 0.999
+ Color: 30; 144; 255
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /perception/traffic_light_recognition/debug/rois
+ Value: /perception/object_recognition/tracking/objects
+ UNKNOWN:
+ Alpha: 0.999
+ Color: 255; 255; 255
Value: true
- - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Tracking
+ - Class: rviz_common/Group
+ Displays:
+ - BUS:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ CAR:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ CYCLIST:
+ Alpha: 0.999
+ Color: 119; 11; 32
+ Class: autoware_auto_perception_rviz_plugin/PredictedObjects
+ Display 3d polygon: true
+ Display Label: true
+ Display PoseWithCovariance: false
+ Display Predicted Path Confidence: true
+ Display Predicted Paths: true
+ Display Twist: false
+ Display UUID: true
+ Display Velocity: true
Enabled: true
- Name: MapBasedDetectionResult
- Namespaces:
- {}
+ MOTORCYCLE:
+ Alpha: 0.999
+ Color: 119; 11; 32
+ Name: PredictedObjects
+ Namespaces: {}
+ PEDESTRIAN:
+ Alpha: 0.999
+ Color: 255; 192; 203
+ TRAILER:
+ Alpha: 0.999
+ Color: 30; 144; 255
+ TRUCK:
+ Alpha: 0.999
+ Color: 30; 144; 255
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers
+ Value: /perception/object_recognition/objects
+ UNKNOWN:
+ Alpha: 0.999
+ Color: 255; 255; 255
Value: true
Enabled: true
- Name: TrafficLight
+ Name: Prediction
- Class: rviz_common/Group
Displays:
- - Alpha: 0.5
- Class: rviz_default_plugins/Map
- Color Scheme: map
- Draw Behind: false
+ - Class: rviz_default_plugins/Image
Enabled: true
- Name: Map
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: RecognitionResultOnImage
+ Normalize Range: true
Topic:
Depth: 5
Durability Policy: Volatile
- Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /perception/occupancy_grid_map/map
- Update Topic:
+ Value: /perception/traffic_light_recognition/debug/rois
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MapBasedDetectionResult
+ Namespaces: {}
+ Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /perception/occupancy_grid_map/map_updates
- Use Timestamp: false
+ Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers
Value: true
- Enabled: false
- Name: OccupancyGrid
+ Enabled: true
+ Name: TrafficLight
Enabled: true
Name: Perception
- Class: rviz_common/Group
@@ -1019,201 +971,6 @@ Visualization Manager:
Constant Color: false
Scale: 0.30000001192092896
Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: false
- Name: Arrow
- Namespaces:
- {}
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path
- Value: false
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: Crosswalk
- Namespaces:
- collision line: false
- collision point: false
- factor_text: true
- slow factor_text: true
- slow point: false
- slow polygon line: false
- slow virtual_wall: true
- stop point: false
- stop polygon line: false
- stop_virtual_wall: true
- walkway stop judge range: false
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: Intersection
- Namespaces:
- candidate_collision_ego_lane_polygon: false
- candidate_collision_object_polygons: false
- conflicting_targets: false
- detection_area: false
- ego_lane: false
- factor_text: true
- judge_point_pose_line: false
- path_raw: false
- spline: false
- stop_point_pose_line: false
- stop_virtual_wall: true
- stuck_vehicle_detect_area: false
- stuck_targets: false
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: MergeFromPrivate
- Namespaces:
- factor_text: true
- stop_point_pose_line: false
- stop_virtual_wall: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/merge_from_private
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: Blind Spot
- Namespaces:
- conflict_area_for_blind_spot: false
- conflicting_targets: false
- detection_area: false
- detection_area_for_blind_spot: false
- factor_text: true
- judge_point_pose_line: false
- path_raw: false
- stop_virtual_wall: true
- spline: false
- stop_point_pose_line: false
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: TrafficLight
- Namespaces:
- dead line factor_text: false
- dead line virtual_wall: false
- factor_text: true
- stop_virtual_wall: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: VirtualTrafficLight
- Namespaces:
- end_lines: false
- instrument_center: false
- instrument_id: false
- start_line: false
- stop_factor_text: true
- stop_line: false
- stop_virtual_wall: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: StopLine
- Namespaces:
- factor_text: true
- stop_virtual_wall: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: DetectionArea
- Namespaces:
- detection_area_correspondence: false
- detection_area_id: false
- detection_area_polygon: false
- factor_text: true
- obstacles: false
- stop_virtual_wall: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: OcclusionSpot
- Namespaces:
- arrow: false
- occlusion spot slow down: true
- collision: false
- info_obstacle: false
- obstacle: false
- detection_area: false
- slow factor_text: true
- path_raw: false
- path_interpolated: false
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: NoStoppingArea
- Namespaces:
- no_stopping_area_correspondence: false
- no_stopping_area_id: false
- no_stopping_area_polygon: false
- stuck_vehicle_detect_area: false
- stop_line_detect_area: false
- stuck_points: false
- stop_factor_text: true
- stop_virtual_wall: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area
- Value: true
- Class: rviz_plugins/Path
Color Border Vel Max: 3
Enabled: true
@@ -1237,18 +994,279 @@ Visualization Manager:
Constant Color: false
Scale: 0.30000001192092896
Value: false
- - Class: rviz_default_plugins/MarkerArray
+ - Class: rviz_common/Group
+ Displays:
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (BlindSpot)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (Crosswalk)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (DetectionArea)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (Intersection)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (MergeFromPrivateArea)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (NoStoppingArea)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (OcclusionSpot)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (StopLine)
+ Namespaces:
+ stop_factor_text: true
+ stop_virtual_wall: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (TrafficLight)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (VirtualTrafficLight)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (RunOut)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out
+ Value: true
+ Enabled: true
+ Name: VirtualWall
+ - Class: rviz_common/Group
+ Displays:
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Arrow
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Crosswalk
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Intersection
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Blind Spot
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: TrafficLight
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: VirtualTrafficLight
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: StopLine
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: DetectionArea
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: OcclusionSpot
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: NoStoppingArea
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area
+ Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: RunOut
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out
+ Value: false
Enabled: false
- Name: DrivableAreaBoundary
- Namespaces:
- {}
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_boundary
- Value: true
+ Name: DebugMarker
Enabled: true
Name: BehaviorPlanning
- Class: rviz_common/Group
@@ -1276,81 +1294,86 @@ Visualization Manager:
Constant Color: false
Scale: 0.30000001192092896
Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: ObstacleStop
- Namespaces:
- collision_polygons: false
- detection_polygons: false
- slow_down_polygons: false
- slow_down_detection_polygons: false
- stop_obstacle_point: false
- stop_obstacle_text: false
- slow_down_obstacle_point: false
- slow_down_obstacle_text: false
- slow_down_start_virtual_wall: false
- slow_down_start_factor_text: false
- slow_down_end_virtual_wall: false
- slow_down_end_factor_text: false
- slow_down_end: false
- stop_virtual_wall: true
- stop_factor_text: true
- slow_down_virtual_wall: true
- slow_down_factor_text: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
- Value: true
- - Class: rviz_default_plugins/MarkerArray
+ - Class: rviz_common/Group
+ Displays:
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (ObstacleStop)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (SurroundObstacle)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (ObstacleAvoidance)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker
+ Value: true
Enabled: true
- Name: SurroundObstacleCheck
- Namespaces:
- factor_text: true
- virtual_wall: true
- obstacle_point: true
- no_start_obstacle_text: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: ObstacleAvoidance
- Namespaces:
- base_bounds_0: false
- base_bounds_1: false
- base_bounds_2: false
- current_vehicle_circles: false
- lateral_errors: false
- mpt_footprints: false
- vehicle_circle_lines: false
- vehicle_circles: false
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker
- Value: true
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: ObstacleAvoidanceWall
- Namespaces:
- virtual_wall: true
- virtual_wall_text: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker
- Value: true
+ Name: VirtualWall
+ - Class: rviz_common/Group
+ Displays:
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: ObstacleStop
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: SurroundObstacleCheck
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: ObstacleAvoidance
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker
+ Value: true
+ Enabled: false
+ Name: DebugMarker
Enabled: true
Name: MotionPlanning
Enabled: true
@@ -1419,22 +1442,6 @@ Visualization Manager:
Value: true
Enabled: true
Name: Parking
- - Class: rviz_common/Group
- Displays:
- - Class: rviz_default_plugins/MarkerArray
- Enabled: true
- Name: TrajectoryErrorMarker
- Namespaces:
- trajectory_relative_angle: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker
- Value: true
- Enabled: true
- Name: PlanningDiagnostics
Enabled: true
Name: ScenarioPlanning
Enabled: true
@@ -1467,8 +1474,7 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Debug/MPC
- Namespaces:
- {}
+ Namespaces: {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -1479,14 +1485,13 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Debug/PurePursuit
- Namespaces:
- {}
+ Namespaces: {}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /control/trajectory_follower/pure_pursuit_node_exe/debug/markers
+ Value: /control/trajectory_follower/pure_pursuit/debug/markers
Value: false
Enabled: true
Name: Control
diff --git a/localization_launch/launch/localization.launch.xml b/localization_launch/launch/localization.launch.xml
index 20b29e5af..91fbe6ac9 100644
--- a/localization_launch/launch/localization.launch.xml
+++ b/localization_launch/launch/localization.launch.xml
@@ -1,6 +1,6 @@
-
+
diff --git a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
index b8781ebac..520892e3d 100644
--- a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
+++ b/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -48,8 +48,11 @@ def __init__(self, context):
self.use_time_series_filter = self.ground_segmentation_param["use_time_series_filter"]
def get_vehicle_info(self):
- # TODO: need to rename key from "ros_params" to "global_params" after Humble
+ # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
+ # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py
gp = self.context.launch_configurations.get("ros_params", {})
+ if not gp:
+ gp = dict(self.context.launch_configurations.get("global_params", {}))
p = {}
p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"]
p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"]
diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
index 462a89542..64252eaf7 100644
--- a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
+++ b/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
@@ -38,7 +38,7 @@
-
+
diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
new file mode 100644
index 000000000..1332e422c
--- /dev/null
+++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
@@ -0,0 +1,18 @@
+/**:
+ ros__parameters:
+ launch_stop_line: true
+ launch_crosswalk: true
+ launch_traffic_light: true
+ launch_intersection: true
+ launch_blind_spot: true
+ launch_detection_area: true
+ launch_virtual_traffic_light: true
+ launch_occlusion_spot: true
+ launch_no_stopping_area: true
+ launch_run_out: false
+ forward_path_length: 1000.0
+ backward_path_length: 5.0
+ max_accel: -2.8
+ max_jerk: -5.0
+ system_delay: 0.5
+ delay_response_time: 1.3
diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
new file mode 100644
index 000000000..8818ed019
--- /dev/null
+++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
@@ -0,0 +1,45 @@
+/**:
+ ros__parameters:
+ run_out:
+ detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points
+ use_partition_lanelet: true # [-] whether to use partition lanelet map data
+ specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates
+ stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin
+ passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
+ deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles
+ obstacle_velocity_kph: 5.0 # [km/h] assumption for obstacle velocity
+ detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles
+ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time
+ min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision
+
+ # rectangle detection area for Points method
+ # this will be replaced with more appropriate detection area
+ detection_area_size:
+ dist_ahead: 50.0 # [m] ahead distance from ego position
+ dist_behind: 5.0 # [m] behind distance from ego position
+ dist_right: 7.0 # [m] right distance from ego position
+ dist_left: 7.0 # [m] left distance from ego position
+
+ # parameter to create abstracted dynamic obstacles
+ dynamic_obstacle:
+ min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles
+ max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles
+ diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points
+ height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points
+ max_prediction_time: 10.0 # [sec] create predicted path until this time
+ time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
+
+ # approach if ego has stopped in the front of the obstacle for a certain amount of time
+ approaching:
+ enable: false
+ margin: 0.0 # [m] distance on how close ego approaches the obstacle
+ limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping
+ stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
+ stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
+ dist_thresh: 0.5 # [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh
+
+ # parameter to avoid sudden stopping
+ slow_down_limit:
+ enable: true
+ max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake.
+ max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake.
diff --git a/planning_launch/launch/planning.launch.xml b/planning_launch/launch/planning.launch.xml
index 640b3a234..f6a7abec7 100644
--- a/planning_launch/launch/planning.launch.xml
+++ b/planning_launch/launch/planning.launch.xml
@@ -1,5 +1,10 @@
+
+
+
+
+
@@ -13,6 +18,8 @@
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving.launch.xml b/planning_launch/launch/scenario_planning/lane_driving.launch.xml
index e5d4d4c6b..ad3e35149 100644
--- a/planning_launch/launch/scenario_planning/lane_driving.launch.xml
+++ b/planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -2,6 +2,10 @@
+
+
+
+
@@ -10,6 +14,8 @@
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index 8e2fe2629..931396032 100644
--- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -18,10 +18,13 @@
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
from launch.actions import SetLaunchConfiguration
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PythonExpression
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch_ros.substitutions import FindPackageShare
@@ -190,7 +193,7 @@ def generate_launch_description():
with open(common_param_path, "r") as f:
common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- base_param_path = os.path.join(
+ motion_velocity_smoother_param_path = os.path.join(
get_package_share_directory("planning_launch"),
"config",
"scenario_planning",
@@ -198,10 +201,10 @@ def generate_launch_description():
"motion_velocity_smoother",
"motion_velocity_smoother.param.yaml",
)
- with open(base_param_path, "r") as f:
- base_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(motion_velocity_smoother_param_path, "r") as f:
+ motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- smoother_param_path = os.path.join(
+ smoother_type_param_path = os.path.join(
get_package_share_directory("planning_launch"),
"config",
"scenario_planning",
@@ -209,8 +212,8 @@ def generate_launch_description():
"motion_velocity_smoother",
"Analytical.param.yaml",
)
- with open(smoother_param_path, "r") as f:
- smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(smoother_type_param_path, "r") as f:
+ smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
# behavior velocity planner
blind_spot_param_path = os.path.join(
@@ -321,6 +324,30 @@ def generate_launch_description():
with open(no_stopping_area_param_path, "r") as f:
no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ run_out_param_path = os.path.join(
+ get_package_share_directory("planning_launch"),
+ "config",
+ "scenario_planning",
+ "lane_driving",
+ "behavior_planning",
+ "behavior_velocity_planner",
+ "run_out.param.yaml",
+ )
+ with open(run_out_param_path, "r") as f:
+ run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+ behavior_velocity_planner_param_path = os.path.join(
+ get_package_share_directory("planning_launch"),
+ "config",
+ "scenario_planning",
+ "lane_driving",
+ "behavior_planning",
+ "behavior_velocity_planner",
+ "behavior_velocity_planner.param.yaml",
+ )
+ with open(behavior_velocity_planner_param_path, "r") as f:
+ behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
behavior_velocity_planner_component = ComposableNode(
package="behavior_velocity_planner",
plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode",
@@ -335,6 +362,10 @@ def generate_launch_description():
"~/input/no_ground_pointcloud",
"/perception/obstacle_segmentation/pointcloud",
),
+ (
+ "~/input/compare_map_filtered_pointcloud",
+ "compare_map_filtered/pointcloud",
+ ),
(
"~/input/traffic_signals",
"/perception/traffic_light_recognition/traffic_signals",
@@ -361,24 +392,10 @@ def generate_launch_description():
("~/output/traffic_signal", "debug/traffic_signal"),
],
parameters=[
- {
- "launch_stop_line": True,
- "launch_crosswalk": True,
- "launch_traffic_light": True,
- "launch_intersection": True,
- "launch_blind_spot": True,
- "launch_detection_area": True,
- "launch_virtual_traffic_light": True,
- "launch_occlusion_spot": True,
- "launch_no_stopping_area": True,
- "forward_path_length": 1000.0,
- "backward_path_length": 5.0,
- "max_accel": -2.8,
- "delay_response_time": 1.3,
- },
+ behavior_velocity_planner_param,
common_param,
- base_param,
- smoother_param,
+ motion_velocity_smoother_param,
+ smoother_type_param,
blind_spot_param,
crosswalk_param,
detection_area_param,
@@ -388,6 +405,7 @@ def generate_launch_description():
virtual_traffic_light_param,
occlusion_spot_param,
no_stopping_area_param,
+ run_out_param,
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
@@ -415,6 +433,35 @@ def generate_launch_description():
condition=IfCondition(LaunchConfiguration("use_multithread")),
)
+ # load compare map for run out module
+ load_compare_map = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [
+ FindPackageShare("planning_launch"),
+ "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py",
+ ]
+ ),
+ launch_arguments={
+ "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+ "container_name": LaunchConfiguration("container_name"),
+ "use_multithread": "true",
+ }.items(),
+ # launch compare map only when run_out module is enabled and detection method is Points
+ condition=IfCondition(
+ PythonExpression(
+ [
+ LaunchConfiguration(
+ "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
+ ),
+ " and ",
+ "'",
+ run_out_param["run_out"]["detection_method"],
+ "' == 'Points'",
+ ]
+ )
+ ),
+ )
+
return launch.LaunchDescription(
[
DeclareLaunchArgument(
@@ -426,6 +473,7 @@ def generate_launch_description():
set_container_executable,
set_container_mt_executable,
container,
+ load_compare_map,
ExecuteProcess(
cmd=[
"ros2",
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py
new file mode 100644
index 000000000..fe3e347fd
--- /dev/null
+++ b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py
@@ -0,0 +1,89 @@
+# Copyright 2022 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+ def add_launch_arg(name: str, default_value=None):
+ return DeclareLaunchArgument(name, default_value=default_value)
+
+ set_container_executable = SetLaunchConfiguration(
+ "container_executable",
+ "component_container",
+ condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+ )
+
+ set_container_mt_executable = SetLaunchConfiguration(
+ "container_executable",
+ "component_container_mt",
+ condition=IfCondition(LaunchConfiguration("use_multithread")),
+ )
+
+ composable_nodes = [
+ ComposableNode(
+ package="compare_map_segmentation",
+ plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent",
+ name="voxel_distance_based_compare_map_filter_node",
+ remappings=[
+ ("input", "/perception/obstacle_segmentation/pointcloud"),
+ ("map", "/map/pointcloud_map"),
+ ("output", "compare_map_filtered/pointcloud"),
+ ],
+ parameters=[
+ {
+ "distance_threshold": 0.7,
+ }
+ ],
+ extra_arguments=[
+ {"use_intra_process_comms": False} # this node has QoS of transient local
+ ],
+ ),
+ ]
+
+ compare_map_container = ComposableNodeContainer(
+ name=LaunchConfiguration("container_name"),
+ namespace="",
+ package="rclcpp_components",
+ executable=LaunchConfiguration("container_executable"),
+ composable_node_descriptions=composable_nodes,
+ condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+ output="screen",
+ )
+
+ load_composable_nodes = LoadComposableNodes(
+ composable_node_descriptions=composable_nodes,
+ target_container=LaunchConfiguration("container_name"),
+ condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+ )
+
+ return LaunchDescription(
+ [
+ add_launch_arg("use_multithread", "true"),
+ add_launch_arg("use_pointcloud_container", "true"),
+ add_launch_arg("container_name", "compare_map_container"),
+ set_container_executable,
+ set_container_mt_executable,
+ compare_map_container,
+ load_composable_nodes,
+ ]
+ )
diff --git a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index 5304034d2..538607f04 100644
--- a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -2,6 +2,10 @@
+
+
+
+
@@ -39,6 +43,8 @@
+
+