diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
new file mode 100644
index 000000000..30faaaeab
--- /dev/null
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -0,0 +1,15 @@
+## PRの種類
+
+- [ ] 新機能
+- [ ] 既存機能の性能向上
+- [ ] バグフィックス
+
+## Jiraリンク
+
+## 変更概要
+
+## レビュー方法
+
+## その他
+
+- [ ] [リリースノート](https://tier4.atlassian.net/wiki/spaces/AIP/pages/563774416)への記載
diff --git a/README.md b/README.md
new file mode 100644
index 000000000..a895a99a5
--- /dev/null
+++ b/README.md
@@ -0,0 +1,2 @@
+# autoware_launcher
+launch files for autoware
diff --git a/autoware_launch/CMakeLists.txt b/autoware_launch/CMakeLists.txt
new file mode 100644
index 000000000..87cc71d21
--- /dev/null
+++ b/autoware_launch/CMakeLists.txt
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(autoware_launch)
+
+find_package(catkin REQUIRED COMPONENTS
+ vehicle_launch
+ perception_launch
+ sensing_launch
+)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ launch
+ rviz
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/autoware_launch/launch/autoware.launch b/autoware_launch/launch/autoware.launch
new file mode 100644
index 000000000..04b4031da
--- /dev/null
+++ b/autoware_launch/launch/autoware.launch
@@ -0,0 +1,61 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/autoware_launch/launch/planning_simulator.launch b/autoware_launch/launch/planning_simulator.launch
new file mode 100644
index 000000000..daa7844ba
--- /dev/null
+++ b/autoware_launch/launch/planning_simulator.launch
@@ -0,0 +1,46 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/autoware_launch/package.xml b/autoware_launch/package.xml
new file mode 100644
index 000000000..0e2c841c6
--- /dev/null
+++ b/autoware_launch/package.xml
@@ -0,0 +1,29 @@
+
+
+ autoware_launch
+ 0.1.0
+ The autoware_launch package
+
+ Yukihiro Saito
+ Apache 2
+
+ catkin
+ vehicle_launch
+ perception_launch
+ sensing_launch
+ vehicle_launch
+ perception_launch
+ sensing_launch
+
+ rosbridge_server
+ python-tornado
+ python3-tornado
+ python-bson
+ python3-bson
+
+
+
+
+
+
+
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
new file mode 100644
index 000000000..6f95da8ae
--- /dev/null
+++ b/autoware_launch/rviz/autoware.rviz
@@ -0,0 +1,1032 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
+ - /Sensing1/GNSS1/PoseWithCovariance1/Covariance1/Position1
+ - /Localization1/NDT1/PoseHistory1/Line1
+ - /Localization1/NDT1/MonteCarloInitialPose1/Namespaces1
+ - /Localization1/EKF1/PoseHistory1/Line1
+ - /Planning1/ScenarioPlanning1
+ - /Control1/Debug/MPC1/Namespaces1
+ Splitter Ratio: 0.557669460773468
+ Tree Height: 435
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /2D Dummy Pedestrian1
+ - /2D Dummy Car1
+ - /2D Checkpoint Pose1
+ - /Delete All Objects1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: PointCloudMap
+ - Class: autoware_localization_rviz_plugin/InitialPoseButtonPanel
+ Name: InitialPoseButtonPanel
+ - Class: autoware_localization_rviz_plugin/InitialPoseButtonPanel
+ Name: InitialPoseButtonPanel
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree: {}
+ Update Interval: 0
+ Value: false
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: false
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: false
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz_plugins/SteeringAngle
+ Enabled: true
+ Left: 128
+ Length: 256
+ Name: SteeringAngle
+ Scale: 17
+ Text Color: 25; 255; 240
+ Top: 128
+ Topic: /vehicle/status/steering
+ Unreliable: false
+ Value: true
+ 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: /vehicle/status/twist
+ Unreliable: false
+ Value: true
+ Value height offset: 0
+ - Alpha: 1
+ Class: rviz_plugins/VelocityHistory
+ Color Border Vel Max: 3
+ Constant Color:
+ Color: 255; 255; 255
+ Value: true
+ Enabled: true
+ Name: VelocityHistory
+ Scale: 0.30000001192092896
+ Timeout: 10
+ Topic: /vehicle/status/twist
+ Unreliable: false
+ Value: true
+ - Alpha: 0.30000001192092896
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera1/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera1/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera2/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera2/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera3/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera3/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera4/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera4/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera5/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera5/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera6/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera6/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ gnss_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tamagawa/imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ traffic_light/camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ traffic_light/camera_optical_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ velodyne_left:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ velodyne_left_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ velodyne_right:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ velodyne_right_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ velodyne_top:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ velodyne_top_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: VehicleModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/PolarGridDisplay
+ Color: 255; 255; 255
+ Delta Range: 10
+ Enabled: true
+ Max Alpha: 1
+ Max Range: 100
+ Max Wave Alpha: 1
+ Min Alpha: 0.20000000298023224
+ Min Wave Alpha: 0.20000000298023224
+ Name: PolarGridDisplay
+ Reference Frame: base_link
+ Value: true
+ Wave Color: 255; 255; 255
+ Wave Velocity: 40
+ - Class: rviz_plugins/TurnSignal
+ Enabled: true
+ Height: 256
+ Left: 196
+ Name: TurnSignal
+ Top: 350
+ Topic: /vehicle/turn_signal_cmd
+ Unreliable: false
+ Value: true
+ Width: 512
+ Enabled: true
+ Name: Vehicle
+ Enabled: true
+ Name: System
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 0.20000000298023224
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 28.71826171875
+ Min Value: -7.4224700927734375
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 237
+ Min Color: 211; 215; 207
+ Min Intensity: 0
+ Name: PointCloudMap
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 1
+ Size (m): 0.05000000074505806
+ Style: Points
+ Topic: /map/pointcloud_map
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: false
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /map/vector_map_marker
+ Name: Lanelet2VectorMap
+ Namespaces:
+ center_lane_line: true
+ crosswalk_lanelets: true
+ lanelet direction: true
+ left_lane_bound: true
+ parking_lots: true
+ parking_space: true
+ right_lane_bound: true
+ road_lanelets: false
+ stop_lines: true
+ traffic_light: true
+ traffic_light_triangle: true
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Name: Map
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 30
+ Min Value: -2
+ Value: false
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: AxisColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: ConcatenatePointCloud
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 2
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /sensing/lidar/concatenated/pointcloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: false
+ Autocompute Value Bounds:
+ Max Value: 15
+ Min Value: -2
+ Value: false
+ Axis: Z
+ Channel Name: z
+ Class: rviz/PointCloud2
+ Color: 0; 240; 255
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 15
+ Min Color: 0; 0; 0
+ Min Intensity: -5
+ Name: NoGroundPointCloud
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 10
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /sensing/lidar/no_ground/pointcloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Class: rviz/Polygon
+ Color: 25; 255; 0
+ Enabled: false
+ Name: MesurementRange
+ Topic: /sensing/lidar/crop_box_filter/crop_box_polygon
+ Unreliable: false
+ Value: false
+ Enabled: true
+ Name: LiDAR
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 233; 185; 110
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: false
+ Position:
+ Alpha: 0.20000000298023224
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.699999988079071
+ Head Radius: 1.2000000476837158
+ Name: PoseWithCovariance
+ Shaft Length: 1
+ Shaft Radius: 0.5
+ Shape: Arrow
+ Topic: /sensing/gnss/pose_with_covariance
+ Unreliable: false
+ Value: true
+ Enabled: false
+ Name: GNSS
+ Enabled: true
+ Name: Sensing
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 0; 170; 255
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.4000000059604645
+ Head Radius: 0.6000000238418579
+ Name: PoseWithCovInitial
+ Shaft Length: 0.6000000238418579
+ Shaft Radius: 0.30000001192092896
+ Shape: Arrow
+ Topic: /localization/pose_estimator/initial_pose_with_covariance
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 0; 255; 0
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.4000000059604645
+ Head Radius: 0.6000000238418579
+ Name: PoseWithCovAligned
+ Shaft Length: 0.6000000238418579
+ Shaft Radius: 0.30000001192092896
+ Shape: Arrow
+ Topic: /localization/pose_estimator/pose_with_covariance
+ Unreliable: false
+ Value: true
+ - Buffer Size: 200
+ Class: rviz_plugins::PoseHistory
+ Enabled: true
+ Line:
+ Color: 170; 255; 127
+ Value: true
+ Width: 0.10000000149011612
+ Name: PoseHistory
+ Topic: /localization/pose_estimator/pose
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 0; 255; 255
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: Initial
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 10
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /localization/util/downsample/pointcloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 85; 255; 0
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 85; 255; 127
+ Max Intensity: 0
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: Aligned
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 10
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /localization/pose_estimator/points_aligned
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /localization/pose_estimator/monte_carlo_initial_pose_marker
+ Name: MonteCarloInitialPose
+ Namespaces: {}
+ Queue Size: 1
+ Value: true
+ Enabled: true
+ Name: NDT
+ - Class: rviz/Group
+ Displays:
+ - Buffer Size: 1000
+ Class: rviz_plugins::PoseHistory
+ Enabled: true
+ Line:
+ Color: 0; 255; 255
+ Value: true
+ Width: 0.10000000149011612
+ Name: PoseHistory
+ Topic: /localization/pose_twist_fusion_filter/pose
+ Value: true
+ Enabled: true
+ Name: EKF
+ Enabled: true
+ Name: Localization
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /perception/object_recognition/detection/objects/visualization
+ Name: DynamicObjects
+ Namespaces: {}
+ Queue Size: 100
+ Value: true
+ Enabled: false
+ Name: Detection
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /perception/object_recognition/tracking/objects/visualization
+ Name: DynamicObjects
+ Namespaces: {}
+ Queue Size: 100
+ Value: true
+ Enabled: false
+ Name: Tracking
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /perception/object_recognition/objects/visualization
+ Name: DynamicObjects
+ Namespaces:
+ label: true
+ path: true
+ path confidence: true
+ shape: true
+ twist: true
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Name: Prediction
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /perception/traffic_light_recognition/debug/rois
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers
+ Name: Beam
+ Namespaces: {}
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Name: TrafficLight
+ Enabled: true
+ Name: Perception
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/mission_planning/route_marker
+ Name: RouteArea
+ Namespaces:
+ goal_lanelets: true
+ left_lane_bound: true
+ right_lane_bound: true
+ route_lanelets: true
+ Queue Size: 100
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.30000001192092896
+ Class: rviz/Pose
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.5
+ Name: GoalPose
+ Shaft Length: 3
+ Shaft Radius: 0.20000000298023224
+ Shape: Axes
+ Topic: /planning/mission_planning/goal
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Name: MissionPlanning
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz_plugins/Trajectory
+ Color Border Vel Max: 3
+ Enabled: true
+ Name: ScenarioTrajectory
+ Topic: /planning/scenario_planning/trajectory
+ Unreliable: false
+ Value: true
+ View Path:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: false
+ Value: true
+ Width: 2
+ View Velocity:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: false
+ Scale: 0.30000001192092896
+ Value: true
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz_plugins/Path
+ Color Border Vel Max: 3
+ Enabled: true
+ Name: Path
+ Topic: /planning/scenario_planning/lane_driving/behavior_planning/path
+ Unreliable: false
+ Value: true
+ View Path:
+ Alpha: 0.4000000059604645
+ Color: 0; 0; 0
+ Constant Color: false
+ Value: true
+ Width: 2
+ View Velocity:
+ Alpha: 0.4000000059604645
+ Color: 0; 0; 0
+ Constant Color: false
+ Scale: 0.30000001192092896
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path
+ Name: Arrow
+ Namespaces: {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk
+ Name: Crosswalk
+ Namespaces: {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
+ Name: Intersection
+ Namespaces:
+ conflicting_targets: true
+ detection_area: false
+ ego_lane: false
+ factor_text: true
+ judge_point_pose_line: true
+ path_raw: false
+ spline: false
+ stop_point_pose_line: false
+ stop_virtual_wall: true
+ stuck_vehicle_detect_area: false
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot
+ Name: Blind Spot
+ Namespaces:
+ conflict_area_for_blind_spot: false
+ conflicting_targets: true
+ detection_area: false
+ detection_area_for_blind_spot: false
+ factor_text: true
+ judge_point_pose_line: true
+ path_raw: false
+ stop_virtual_wall: true
+ spline: false
+ stop_point_pose_line: false
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light
+ Name: TrafficLight
+ Namespaces: {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line
+ Name: StopLine
+ Namespaces:
+ factor_text: true
+ stop_virtual_wall: true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
+ Name: DetectionArea
+ Namespaces:
+ factor_text: true
+ stop_virtual_wall: true
+ Queue Size: 100
+ Value: true
+ - Class: rviz_plugins/Path
+ Color Border Vel Max: 3
+ Enabled: true
+ Name: LaneChangeCandidate
+ Topic: /planning/scenario_planning/lane_driving/lane_change_candidate_path
+ Unreliable: false
+ Value: true
+ View Path:
+ Alpha: 0.30000001192092896
+ Color: 115; 210; 22
+ Constant Color: true
+ Value: true
+ Width: 2
+ View Velocity:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: true
+ Scale: 0.30000001192092896
+ Value: false
+ Enabled: true
+ Name: BehaviorPlanning
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz_plugins/Trajectory
+ Color Border Vel Max: 3
+ Enabled: false
+ Name: Trajectory
+ Topic: /planning/scenario_planning/lane_driving/trajectory
+ Unreliable: false
+ Value: false
+ View Path:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: false
+ Value: true
+ Width: 2
+ View Velocity:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: false
+ Scale: 0.30000001192092896
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
+ Name: ObstaclePointCloudStop
+ Namespaces:
+ collision_polygons: true
+ detection_polygons: true
+ factor_text: true
+ stop_virtual_wall: true
+ stop_obstacle_point: true
+ stop_obstacle_text: true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker
+ Name: SurroundObstacleCheck
+ Namespaces:
+ factor_text: true
+ virtual_wall: true
+ obstacle_point: true
+ no_start_obstacle_text: true
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Name: MotionPlanning
+ Enabled: true
+ Name: LaneDriving
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: false
+ Name: Costmap
+ Topic: /planning/scenario_planning/parking/costmap_generator/occupancy_grid
+ Unreliable: false
+ Use Timestamp: false
+ Value: false
+ - Alpha: 1
+ Arrow Length: 0.30000001192092896
+ Axes Length: 0.30000001192092896
+ Axes Radius: 0.05000000074505806
+ Class: rviz/PoseArray
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.10000000149011612
+ Head Radius: 0.20000000298023224
+ Name: PartialPoseArray
+ Shaft Length: 0.20000000298023224
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow (3D)
+ Topic: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Arrow Length: 0.5
+ Axes Length: 0.30000001192092896
+ Axes Radius: 0.009999999776482582
+ Class: rviz/PoseArray
+ Color: 0; 0; 255
+ Enabled: true
+ Head Length: 0.10000000149011612
+ Head Radius: 0.20000000298023224
+ Name: PoseArray
+ Shaft Length: 0.20000000298023224
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow (Flat)
+ Topic: /planning/scenario_planning/parking/freespace_planner/debug/pose_array
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Name: Parking
+ Enabled: true
+ Name: ScenarioPlanning
+ Enabled: true
+ Name: Planning
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /control/mpc_follower/debug/markers
+ Name: Debug/MPC
+ Namespaces: {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /control/pure_pursuit/debug/marker
+ Name: Debug/PurePursuit
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ Enabled: true
+ Name: Control
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: viewer
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /planning/mission_planning/goal
+ - Class: rviz/PedestrianInitialPoseTool
+ Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Theta std deviation: 0.0872664600610733
+ Velocity: 1
+ X std deviation: 0.009999999776482582
+ Y std deviation: 0.009999999776482582
+ Z position: 0
+ Z std deviation: 0.009999999776482582
+ - Class: rviz/CarInitialPoseTool
+ Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Theta std deviation: 0.0872664600610733
+ Velocity: 3
+ X std deviation: 0.009999999776482582
+ Y std deviation: 0.009999999776482582
+ Z position: 0
+ Z std deviation: 0.009999999776482582
+ - Class: rviz/MissionCheckpointTool
+ Pose Topic: /planning/mission_planning/checkpoint
+ Theta std deviation: 0.2617993950843811
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ Z position: 0
+ - Class: rviz/DeleteAllObjectsTool
+ Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Value: true
+ Views:
+ Current:
+ Angle: 0
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Scale: 10
+ Target Frame: viewer
+ Value: TopDownOrtho (rviz)
+ X: 0
+ Y: 0
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1565
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ InitialPoseButtonPanel:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000033c00000563fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f0000000c900fffffffb0000000a005600690065007700730100000233000000f2000000a400fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000032b000000a10000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d00610067006501000003d2000001ce0000001600fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700ffffff000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000afd0000005afc0100000002fb0000000800540069006d0065010000000000000afd000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007bb0000056300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 2813
+ X: 67
+ Y: 27
diff --git a/control_launch/CMakeLists.txt b/control_launch/CMakeLists.txt
new file mode 100644
index 000000000..e469a1831
--- /dev/null
+++ b/control_launch/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(control_launch)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ config
+ launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/control_launch/config/mpc_follower/mpc_follower_param.yaml b/control_launch/config/mpc_follower/mpc_follower_param.yaml
new file mode 100644
index 000000000..cb55f9242
--- /dev/null
+++ b/control_launch/config/mpc_follower/mpc_follower_param.yaml
@@ -0,0 +1,40 @@
+
+
+# -- system --
+ctrl_period: 0.03 # control period [s]
+traj_resample_dist: 0.1 # path resampling interval [m]
+enable_yaw_recalculation: false # flag for recalculation of yaw angle after resampling
+admisible_position_error: 5.0 # stop mpc calculation when error is larger than the following value
+admisible_yaw_error_: 1.57 # stop mpc calculation when error is larger than the following value
+
+# -- path smoothing --
+enable_path_smoothing: false # flag for path smoothing
+path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
+curvature_smoothing_num: 15 # point-to-point index distance used in curvature calculation : curvature is calculated from three points p(i-num), p(i), p(i+num)
+
+# -- mpc optimization --
+qpoases_max_iter: 500 # max iteration number for quadratic programming
+qp_solver_type: "osqp" # optimization solver type. option is unconstraint_fast, unconstraint, and qpoases_hotstart and osqp
+mpc_prediction_horizon: 50 # prediction horizon step
+mpc_prediction_dt: 0.1 # prediction horizon period [s]
+mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q
+mpc_weight_heading_error: 0.0 # heading error weight in matrix Q
+mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q
+mpc_weight_steering_input: 1.0 # steering error weight in matrix R
+mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R
+mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R
+mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R
+mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R
+mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability
+mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability
+mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero
+
+# -- vehicle model --
+vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
+input_delay: 0.24 # steering input delay time for delay compensation
+vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approzimation) [s]
+steer_lim_deg: 40.0 # steering angle limit [deg]
+steer_rate_lim_deg: 600.0 # steering angle rate limit [deg/s]
+
+# -- lowpass filter for noise reduction --
+steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz]
diff --git a/control_launch/config/pure_pursuit/pure_pursuit_param.yaml b/control_launch/config/pure_pursuit/pure_pursuit_param.yaml
new file mode 100644
index 000000000..9d38aefc3
--- /dev/null
+++ b/control_launch/config/pure_pursuit/pure_pursuit_param.yaml
@@ -0,0 +1,7 @@
+# -- system --
+control_period: 0.033
+
+# --algorithm
+lookahead_distance_ratio: 2.2
+min_lookahead_distance: 2.5
+reverse_min_lookahead_distance: 7.0
diff --git a/control_launch/launch/control.launch b/control_launch/launch/control.launch
new file mode 100644
index 000000000..c2ee293fa
--- /dev/null
+++ b/control_launch/launch/control.launch
@@ -0,0 +1,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/control_launch/package.xml b/control_launch/package.xml
new file mode 100644
index 000000000..c2f915721
--- /dev/null
+++ b/control_launch/package.xml
@@ -0,0 +1,22 @@
+
+
+ control_launch
+ 0.1.0
+ The control_launch package
+
+
+
+
+ Takamasa Horibe
+
+
+
+
+
+ TODO
+
+ catkin
+
+
+
+
diff --git a/localization_launch/CMakeLists.txt b/localization_launch/CMakeLists.txt
new file mode 100644
index 000000000..047892fac
--- /dev/null
+++ b/localization_launch/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(localization_launch)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/localization_launch/launch/localization.launch b/localization_launch/launch/localization.launch
new file mode 100644
index 000000000..1543c9262
--- /dev/null
+++ b/localization_launch/launch/localization.launch
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/localization_launch/launch/pose_estimator/pose_estimator.launch b/localization_launch/launch/pose_estimator/pose_estimator.launch
new file mode 100644
index 000000000..f70e6d314
--- /dev/null
+++ b/localization_launch/launch/pose_estimator/pose_estimator.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch b/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch
new file mode 100644
index 000000000..76310afb0
--- /dev/null
+++ b/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/localization_launch/launch/twist_estimator/twist_estimator.launch b/localization_launch/launch/twist_estimator/twist_estimator.launch
new file mode 100644
index 000000000..da8198ef7
--- /dev/null
+++ b/localization_launch/launch/twist_estimator/twist_estimator.launch
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
diff --git a/localization_launch/launch/util/util.launch b/localization_launch/launch/util/util.launch
new file mode 100644
index 000000000..a0558ce96
--- /dev/null
+++ b/localization_launch/launch/util/util.launch
@@ -0,0 +1,45 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ min_x: -60.0
+ max_x: 60.0
+ min_y: -60.0
+ max_y: 60.0
+ min_z: -30.0
+ max_z: 50.0
+ negative: False
+
+
+
+
+
+
+
+
+
+
+
+ voxel_size_x: 3.0
+ voxel_size_y: 3.0
+ voxel_size_z: 3.0
+
+
+
+
diff --git a/localization_launch/package.xml b/localization_launch/package.xml
new file mode 100644
index 000000000..48230f21c
--- /dev/null
+++ b/localization_launch/package.xml
@@ -0,0 +1,58 @@
+
+
+ localization_launch
+ 0.1.0
+ The localization_launch package
+
+
+
+
+ Yamato Ando
+
+
+
+
+
+ Apache 2
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+
+
+
+
+
+
+
diff --git a/map_launch/CMakeLists.txt b/map_launch/CMakeLists.txt
new file mode 100644
index 000000000..c318300cf
--- /dev/null
+++ b/map_launch/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(map_launch)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+include_directories()
+
+install(
+ DIRECTORY
+ launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/map_launch/launch/map.launch b/map_launch/launch/map.launch
new file mode 100644
index 000000000..efa73f802
--- /dev/null
+++ b/map_launch/launch/map.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/map_launch/package.xml b/map_launch/package.xml
new file mode 100644
index 000000000..df7aacd22
--- /dev/null
+++ b/map_launch/package.xml
@@ -0,0 +1,59 @@
+
+
+ map_launch
+ 0.1.0
+ The map_launch package
+
+
+
+
+ mitsudome-r
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+
+
+
+
+
+
+
+
diff --git a/perception_launch/CMakeLists.txt b/perception_launch/CMakeLists.txt
new file mode 100644
index 000000000..3617fa63d
--- /dev/null
+++ b/perception_launch/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(perception_launch)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch
new file mode 100644
index 000000000..b35bc1396
--- /dev/null
+++ b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch
@@ -0,0 +1,62 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/launch/object_recognition/detection/detection.launch b/perception_launch/launch/object_recognition/detection/detection.launch
new file mode 100644
index 000000000..f72fa95b3
--- /dev/null
+++ b/perception_launch/launch/object_recognition/detection/detection.launch
@@ -0,0 +1,64 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch
new file mode 100644
index 000000000..6fe87c5e6
--- /dev/null
+++ b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/launch/object_recognition/prediction/prediction.launch b/perception_launch/launch/object_recognition/prediction/prediction.launch
new file mode 100644
index 000000000..efd2535b2
--- /dev/null
+++ b/perception_launch/launch/object_recognition/prediction/prediction.launch
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/launch/object_recognition/tracking/tracking.launch b/perception_launch/launch/object_recognition/tracking/tracking.launch
new file mode 100644
index 000000000..a2fc3947b
--- /dev/null
+++ b/perception_launch/launch/object_recognition/tracking/tracking.launch
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/launch/perception.launch b/perception_launch/launch/perception.launch
new file mode 100644
index 000000000..ee29ebf67
--- /dev/null
+++ b/perception_launch/launch/perception.launch
@@ -0,0 +1,73 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light.launch b/perception_launch/launch/traffic_light_recognition/traffic_light.launch
new file mode 100644
index 000000000..8f16904c1
--- /dev/null
+++ b/perception_launch/launch/traffic_light_recognition/traffic_light.launch
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/perception_launch/package.xml b/perception_launch/package.xml
new file mode 100644
index 000000000..458e108e1
--- /dev/null
+++ b/perception_launch/package.xml
@@ -0,0 +1,27 @@
+
+
+ perception_launch
+ 0.1.0
+ The perception_launch package
+
+ Yukihiro Saito
+ Apache2
+
+
+ catkin
+ dynamic_object_visualization
+ euclidean_cluster
+ pointcloud_to_laserscan
+ shape_estimation
+ dynamic_object_visualization
+ euclidean_cluster
+ pointcloud_to_laserscan
+ shape_estimation
+ dynamic_object_visualization
+ euclidean_cluster
+ pointcloud_to_laserscan
+ shape_estimation
+
+
+
+
diff --git a/planning_launch/CMakeLists.txt b/planning_launch/CMakeLists.txt
new file mode 100644
index 000000000..65406d789
--- /dev/null
+++ b/planning_launch/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(planning_launch)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ launch
+ config
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml
new file mode 100644
index 000000000..c32486719
--- /dev/null
+++ b/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml
@@ -0,0 +1,36 @@
+max_velocity: 20.0 # max velocity limit [m/s]
+max_accel: 1.0 # max acceleration limit [m/ss]
+min_decel: -1.0 # min deceleration limit [m/ss]
+
+# curve parameters
+max_lateral_accel: 0.5 # max lateral acceleration limit [m/ss]
+min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss]
+decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit
+decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit
+
+# engage & replan parameters
+replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s]
+engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
+engage_exit_ratio: 0.5 # exit engage sequence to normal velocity plannning when the velocity exceeds engage_exit_ratio x engage_velocity.
+stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
+
+extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m]
+extract_behind_dist: 5.0 # backward trajectory distance used for planning [m]
+delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajecotory pose [radian]
+
+max_trajectory_length: 200.0 # max trajectory length for resampling [m]
+min_trajectory_length: 30.0 # min trajectory length for resampling [m]
+resample_time: 10.0 # resample total time [s]
+resample_dt: 0.1 # resample time interval [s]
+min_trajectory_interval_distance: 0.1 # resample points-interval length [m]
+
+# default weights
+# L2: jerk = 100.0, v_weight = 100000.0, a_weight = 1000.0
+# Linf: jerk = 200.0, v_weight = 100000.0, a_weight = 5000.0
+
+pseudo_jerk_weight: 100.0 # weight for "smoothness" cost
+over_v_weight: 100000.0 # weight for "overspeed limit" cost
+over_a_weight: 1000.0 # weight for "overaccel limit" cost
+
+algorithm_type: "L2" # Option : L2, Linf
\ No newline at end of file
diff --git a/planning_launch/launch/mission_planning/mission_planning.launch b/planning_launch/launch/mission_planning/mission_planning.launch
new file mode 100644
index 000000000..d1e94f15c
--- /dev/null
+++ b/planning_launch/launch/mission_planning/mission_planning.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/launch/planning.launch b/planning_launch/launch/planning.launch
new file mode 100644
index 000000000..d8d6dd9a1
--- /dev/null
+++ b/planning_launch/launch/planning.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving.launch b/planning_launch/launch/scenario_planning/lane_driving.launch
new file mode 100644
index 000000000..27fa088c4
--- /dev/null
+++ b/planning_launch/launch/scenario_planning/lane_driving.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch
new file mode 100644
index 000000000..cc0e35f6b
--- /dev/null
+++ b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch
@@ -0,0 +1,47 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch
new file mode 100644
index 000000000..8ae766945
--- /dev/null
+++ b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/launch/scenario_planning/parking.launch b/planning_launch/launch/scenario_planning/parking.launch
new file mode 100644
index 000000000..0198eb09d
--- /dev/null
+++ b/planning_launch/launch/scenario_planning/parking.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/launch/scenario_planning/scenario_planning.launch b/planning_launch/launch/scenario_planning/scenario_planning.launch
new file mode 100644
index 000000000..c4d21f910
--- /dev/null
+++ b/planning_launch/launch/scenario_planning/scenario_planning.launch
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning_launch/package.xml b/planning_launch/package.xml
new file mode 100644
index 000000000..24d7f88c6
--- /dev/null
+++ b/planning_launch/package.xml
@@ -0,0 +1,59 @@
+
+
+ planning_launch
+ 0.1.0
+ The planning_launch package
+
+
+
+
+ tier4
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/CMakeLists.txt b/sensing_launch/CMakeLists.txt
new file mode 100644
index 000000000..ab708b8c4
--- /dev/null
+++ b/sensing_launch/CMakeLists.txt
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sensing_launch)
+
+find_package(catkin REQUIRED)
+
+# Force Tier IV's fork version
+find_package(velodyne_pointcloud 0.1.0 EXACT REQUIRED)
+find_package(velodyne_vls_pointcloud 0.1.0 EXACT REQUIRED)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ launch
+ data
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/sensing_launch/data/traffic_light_camera.yaml b/sensing_launch/data/traffic_light_camera.yaml
new file mode 100644
index 000000000..a4c99fae9
--- /dev/null
+++ b/sensing_launch/data/traffic_light_camera.yaml
@@ -0,0 +1,20 @@
+image_width: 1920
+image_height: 1080
+camera_name: traffic_light/camera
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [2410.755261, 0.000000, 922.621401, 0.000000, 2403.573140, 534.752500, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.126600, 0.152594, 0.002432, -0.001244, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [2370.254883, 0.000000, 920.136018, 0.000000, 0.000000, 2388.885254, 535.599668, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
\ No newline at end of file
diff --git a/sensing_launch/launch/aip_customized/camera.launch b/sensing_launch/launch/aip_customized/camera.launch
new file mode 100644
index 000000000..c9e5fa4f2
--- /dev/null
+++ b/sensing_launch/launch/aip_customized/camera.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_customized/gnss.launch b/sensing_launch/launch/aip_customized/gnss.launch
new file mode 100644
index 000000000..51b386c05
--- /dev/null
+++ b/sensing_launch/launch/aip_customized/gnss.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_customized/imu.launch b/sensing_launch/launch/aip_customized/imu.launch
new file mode 100644
index 000000000..b55a51ea8
--- /dev/null
+++ b/sensing_launch/launch/aip_customized/imu.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_customized/lidar.launch b/sensing_launch/launch/aip_customized/lidar.launch
new file mode 100644
index 000000000..0d8fdb308
--- /dev/null
+++ b/sensing_launch/launch/aip_customized/lidar.launch
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [/sensing/lidar/top/outlier_filtered/pointcloud,
+ /sensing/lidar/left/outlier_filtered/pointcloud,
+ /sensing/lidar/right/outlier_filtered/pointcloud]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_s1/camera.launch b/sensing_launch/launch/aip_s1/camera.launch
new file mode 100644
index 000000000..c9e5fa4f2
--- /dev/null
+++ b/sensing_launch/launch/aip_s1/camera.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_s1/gnss.launch b/sensing_launch/launch/aip_s1/gnss.launch
new file mode 100644
index 000000000..51b386c05
--- /dev/null
+++ b/sensing_launch/launch/aip_s1/gnss.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_s1/imu.launch b/sensing_launch/launch/aip_s1/imu.launch
new file mode 100644
index 000000000..b55a51ea8
--- /dev/null
+++ b/sensing_launch/launch/aip_s1/imu.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_s1/lidar.launch b/sensing_launch/launch/aip_s1/lidar.launch
new file mode 100644
index 000000000..0d8fdb308
--- /dev/null
+++ b/sensing_launch/launch/aip_s1/lidar.launch
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [/sensing/lidar/top/outlier_filtered/pointcloud,
+ /sensing/lidar/left/outlier_filtered/pointcloud,
+ /sensing/lidar/right/outlier_filtered/pointcloud]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x1/camera.launch b/sensing_launch/launch/aip_x1/camera.launch
new file mode 100644
index 000000000..c9e5fa4f2
--- /dev/null
+++ b/sensing_launch/launch/aip_x1/camera.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x1/gnss.launch b/sensing_launch/launch/aip_x1/gnss.launch
new file mode 100644
index 000000000..51b386c05
--- /dev/null
+++ b/sensing_launch/launch/aip_x1/gnss.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x1/imu.launch b/sensing_launch/launch/aip_x1/imu.launch
new file mode 100644
index 000000000..b55a51ea8
--- /dev/null
+++ b/sensing_launch/launch/aip_x1/imu.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x1/lidar.launch b/sensing_launch/launch/aip_x1/lidar.launch
new file mode 100644
index 000000000..0d8fdb308
--- /dev/null
+++ b/sensing_launch/launch/aip_x1/lidar.launch
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [/sensing/lidar/top/outlier_filtered/pointcloud,
+ /sensing/lidar/left/outlier_filtered/pointcloud,
+ /sensing/lidar/right/outlier_filtered/pointcloud]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x2/camera.launch b/sensing_launch/launch/aip_x2/camera.launch
new file mode 100644
index 000000000..c9e5fa4f2
--- /dev/null
+++ b/sensing_launch/launch/aip_x2/camera.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x2/gnss.launch b/sensing_launch/launch/aip_x2/gnss.launch
new file mode 100644
index 000000000..51b386c05
--- /dev/null
+++ b/sensing_launch/launch/aip_x2/gnss.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x2/imu.launch b/sensing_launch/launch/aip_x2/imu.launch
new file mode 100644
index 000000000..b55a51ea8
--- /dev/null
+++ b/sensing_launch/launch/aip_x2/imu.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_x2/lidar.launch b/sensing_launch/launch/aip_x2/lidar.launch
new file mode 100644
index 000000000..0d8fdb308
--- /dev/null
+++ b/sensing_launch/launch/aip_x2/lidar.launch
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [/sensing/lidar/top/outlier_filtered/pointcloud,
+ /sensing/lidar/left/outlier_filtered/pointcloud,
+ /sensing/lidar/right/outlier_filtered/pointcloud]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx1/camera.launch b/sensing_launch/launch/aip_xx1/camera.launch
new file mode 100644
index 000000000..0eb3b6b0a
--- /dev/null
+++ b/sensing_launch/launch/aip_xx1/camera.launch
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx1/gnss.launch b/sensing_launch/launch/aip_xx1/gnss.launch
new file mode 100644
index 000000000..51b386c05
--- /dev/null
+++ b/sensing_launch/launch/aip_xx1/gnss.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx1/imu.launch b/sensing_launch/launch/aip_xx1/imu.launch
new file mode 100644
index 000000000..b55a51ea8
--- /dev/null
+++ b/sensing_launch/launch/aip_xx1/imu.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx1/lidar.launch b/sensing_launch/launch/aip_xx1/lidar.launch
new file mode 100644
index 000000000..d57589039
--- /dev/null
+++ b/sensing_launch/launch/aip_xx1/lidar.launch
@@ -0,0 +1,116 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [/sensing/lidar/top/outlier_filtered/pointcloud,
+ /sensing/lidar/left/outlier_filtered/pointcloud,
+ /sensing/lidar/right/outlier_filtered/pointcloud,
+ /sensing/lidar/rear/outlier_filtered/pointcloud]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx2/camera.launch b/sensing_launch/launch/aip_xx2/camera.launch
new file mode 100644
index 000000000..c9e5fa4f2
--- /dev/null
+++ b/sensing_launch/launch/aip_xx2/camera.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx2/gnss.launch b/sensing_launch/launch/aip_xx2/gnss.launch
new file mode 100644
index 000000000..51b386c05
--- /dev/null
+++ b/sensing_launch/launch/aip_xx2/gnss.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx2/imu.launch b/sensing_launch/launch/aip_xx2/imu.launch
new file mode 100644
index 000000000..b55a51ea8
--- /dev/null
+++ b/sensing_launch/launch/aip_xx2/imu.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/aip_xx2/lidar.launch b/sensing_launch/launch/aip_xx2/lidar.launch
new file mode 100644
index 000000000..0d8fdb308
--- /dev/null
+++ b/sensing_launch/launch/aip_xx2/lidar.launch
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [/sensing/lidar/top/outlier_filtered/pointcloud,
+ /sensing/lidar/left/outlier_filtered/pointcloud,
+ /sensing/lidar/right/outlier_filtered/pointcloud]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/livox_horizon.launch b/sensing_launch/launch/livox_horizon.launch
new file mode 100644
index 000000000..aeb1f4431
--- /dev/null
+++ b/sensing_launch/launch/livox_horizon.launch
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/sensing.launch b/sensing_launch/launch/sensing.launch
new file mode 100644
index 000000000..019af2369
--- /dev/null
+++ b/sensing_launch/launch/sensing.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/velodyne_VLP16.launch b/sensing_launch/launch/velodyne_VLP16.launch
new file mode 100644
index 000000000..279c04631
--- /dev/null
+++ b/sensing_launch/launch/velodyne_VLP16.launch
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/velodyne_VLP32C.launch b/sensing_launch/launch/velodyne_VLP32C.launch
new file mode 100644
index 000000000..13ce69c6a
--- /dev/null
+++ b/sensing_launch/launch/velodyne_VLP32C.launch
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/launch/velodyne_VLS128.launch b/sensing_launch/launch/velodyne_VLS128.launch
new file mode 100644
index 000000000..b2833f119
--- /dev/null
+++ b/sensing_launch/launch/velodyne_VLS128.launch
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensing_launch/package.xml b/sensing_launch/package.xml
new file mode 100644
index 000000000..4a87ff366
--- /dev/null
+++ b/sensing_launch/package.xml
@@ -0,0 +1,17 @@
+
+
+ sensing_launch
+ 0.1.0
+ The sensing_launch package
+
+ Yukihiro Saito
+ Apache2
+
+ catkin
+
+ velodyne_pointcloud
+ velodyne_vls_pointcloud
+
+
+
+
diff --git a/system_launch/CMakeLists.txt b/system_launch/CMakeLists.txt
new file mode 100644
index 000000000..2519b3e2f
--- /dev/null
+++ b/system_launch/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(system_launch)
+
+find_package(catkin REQUIRED COMPONENTS
+)
+
+catkin_package()
+
+install(
+ DIRECTORY
+ launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/system_launch/launch/system.launch b/system_launch/launch/system.launch
new file mode 100644
index 000000000..b30703b4f
--- /dev/null
+++ b/system_launch/launch/system.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/system_launch/package.xml b/system_launch/package.xml
new file mode 100644
index 000000000..7f5f70ae0
--- /dev/null
+++ b/system_launch/package.xml
@@ -0,0 +1,14 @@
+
+
+ system_launch
+ 0.1.0
+ The system_launch package
+
+ Kenji Miyake
+ Apache 2
+
+ catkin
+
+
+
+
diff --git a/vehicle_launch/CMakeLists.txt b/vehicle_launch/CMakeLists.txt
new file mode 100644
index 000000000..331b62f49
--- /dev/null
+++ b/vehicle_launch/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vehicle_launch)
+
+add_compile_options(-std=c++14)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY
+ launch
+ config
+ urdf
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/vehicle_launch/config/aip_customized/sensors_calibration.yaml b/vehicle_launch/config/aip_customized/sensors_calibration.yaml
new file mode 100644
index 000000000..e69de29bb
diff --git a/vehicle_launch/config/aip_s1/sensors_calibration.yaml b/vehicle_launch/config/aip_s1/sensors_calibration.yaml
new file mode 100644
index 000000000..e69de29bb
diff --git a/vehicle_launch/config/aip_x1/sensors_calibration.yaml b/vehicle_launch/config/aip_x1/sensors_calibration.yaml
new file mode 100644
index 000000000..e69de29bb
diff --git a/vehicle_launch/config/aip_x2/sensors_calibration.yaml b/vehicle_launch/config/aip_x2/sensors_calibration.yaml
new file mode 100644
index 000000000..e69de29bb
diff --git a/vehicle_launch/config/aip_xx1/sensor_kit_calibration.yaml b/vehicle_launch/config/aip_xx1/sensor_kit_calibration.yaml
new file mode 100644
index 000000000..d9d1f599e
--- /dev/null
+++ b/vehicle_launch/config/aip_xx1/sensor_kit_calibration.yaml
@@ -0,0 +1,91 @@
+sensor_kit_base_link2camera0:
+ x: 0.10731
+ y: 0.56343
+ z: -0.27697
+ roll: -0.025
+ pitch: 0.315
+ yaw: 1.035
+sensor_kit_base_link2camera1:
+ x: -0.10731
+ y: -0.56343
+ z: -0.27697
+ roll: -0.025
+ pitch: 0.32
+ yaw: -2.12
+sensor_kit_base_link2camera2:
+ x: 0.10731
+ y: -0.56343
+ z: -0.27697
+ roll: -0.00
+ pitch: 0.335
+ yaw: -1.04
+sensor_kit_base_link2camera3:
+ x: -0.10731
+ y: 0.56343
+ z: -0.27697
+ roll: 0.0
+ pitch: 0.325
+ yaw: 2.0943951
+sensor_kit_base_link2camera4:
+ x: 0.07356
+ y: 0.0
+ z: -0.0525
+ roll: 0.0
+ pitch: -0.03
+ yaw: -0.005
+sensor_kit_base_link2camera5:
+ x: -0.07356
+ y: 0.0
+ z: -0.0525
+ roll: 0.0
+ pitch: -0.01
+ yaw: 3.125
+sensor_kit_base_link2traffic_light_right_camera:
+ x: 0.05
+ y: -0.0175
+ z: -0.1
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
+sensor_kit_base_link2traffic_light_left_camera:
+ x: 0.05
+ y: 0.0175
+ z: -0.1
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
+sensor_kit_base_link2velodyne_top_base_link:
+ x: 0.0
+ y: 0.0
+ z: 0.0
+ roll: 0.0
+ pitch: 0.0
+ yaw: 1.575
+sensor_kit_base_link2velodyne_left_base_link:
+ x: 0.0
+ y: 0.56362
+ z: -0.30555
+ roll: -0.02
+ pitch: 0.71
+ yaw: 1.575
+sensor_kit_base_link2velodyne_right_base_link:
+ x: 0.0
+ y: -0.56362
+ z: -0.30555
+ roll: -0.01
+ pitch: 0.71
+ yaw: -1.580
+sensor_kit_base_link2gnss:
+ x: -0.1
+ y: 0.0
+ z: -0.2
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
+sensor_kit_base_link2imu_tamagawa:
+ x: 0.0
+ y: 0.0
+ z: 0.0
+ roll: 3.14159265359
+ pitch: 0.0
+ yaw: 3.14159265359
diff --git a/vehicle_launch/config/aip_xx1/sensors_calibration.yaml b/vehicle_launch/config/aip_xx1/sensors_calibration.yaml
new file mode 100644
index 000000000..1aa097239
--- /dev/null
+++ b/vehicle_launch/config/aip_xx1/sensors_calibration.yaml
@@ -0,0 +1,28 @@
+base_link2sensor_kit_base_link:
+ x: 0.9
+ y: 0.0
+ z: 2.0
+ roll: -0.001
+ pitch: 0.015
+ yaw: -0.0364
+base_link2livox_front_right_base_link:
+ x: 3.290
+ y: -0.65485
+ z: 0.3216
+ roll: 0.0
+ pitch: 0.0
+ yaw: -0.872664444
+base_link2livox_front_left_base_link:
+ x: 3.290
+ y: 0.65485
+ z: 0.3016
+ roll: -0.021
+ pitch: 0.05
+ yaw: 0.872664444
+base_link2velodyne_rear_base_link:
+ x: -0.358
+ y: 0.0
+ z: 1.631
+ roll: -0.02
+ pitch: 0.7281317
+ yaw: 3.141592
diff --git a/vehicle_launch/config/aip_xx2/sensors_calibration.yaml b/vehicle_launch/config/aip_xx2/sensors_calibration.yaml
new file mode 100644
index 000000000..e69de29bb
diff --git a/vehicle_launch/launch/vehicle.launch b/vehicle_launch/launch/vehicle.launch
new file mode 100644
index 000000000..653c7b527
--- /dev/null
+++ b/vehicle_launch/launch/vehicle.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/vehicle_launch/launch/vehicle_description.launch b/vehicle_launch/launch/vehicle_description.launch
new file mode 100644
index 000000000..341a2ae8e
--- /dev/null
+++ b/vehicle_launch/launch/vehicle_description.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/vehicle_launch/launch/vehicle_interface.launch b/vehicle_launch/launch/vehicle_interface.launch
new file mode 100644
index 000000000..16706c1ee
--- /dev/null
+++ b/vehicle_launch/launch/vehicle_interface.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/vehicle_launch/package.xml b/vehicle_launch/package.xml
new file mode 100644
index 000000000..457a38c13
--- /dev/null
+++ b/vehicle_launch/package.xml
@@ -0,0 +1,20 @@
+
+
+ vehicle_launch
+ 0.1.0
+ The vehicle_launch package
+
+ Yukihiro Saito
+ Apache2
+
+
+ catkin
+ camera_description
+ imu_description
+ livox_description
+ velodyne_description
+
+ roscpp
+
+
+
diff --git a/vehicle_launch/urdf/vehicle.xacro b/vehicle_launch/urdf/vehicle.xacro
new file mode 100644
index 000000000..30db9c6a7
--- /dev/null
+++ b/vehicle_launch/urdf/vehicle.xacro
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+