From bbc8d87122498a732f384deb60f12d04e969ed09 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 30 Aug 2024 02:41:45 +0900 Subject: [PATCH 01/12] launch costmap_generator Signed-off-by: Autumn60 --- .../planning/costmap_generator.param.yaml | 18 ++++++++++++++++++ .../launch/components/planning.launch.xml | 11 +++++++++++ .../launch/costmap_generator.launch.xml | 3 +++ 3 files changed, 32 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml new file mode 100644 index 00000000..761d16e7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + update_rate: 10.0 + map_frame_id: "map" + costmap_center_frame_id: "base_link" + costmap_width: 50.0 + costmap_resolution: 0.2 + multi_layered_costmap: + layers: + - "lanelet_layer" + - "predicted_object_layer" + lanelet_layer: + type: "lanelet" + map_topic: "/map/vector_map" + predicted_object_layer: + type: "predicted_object" + predicted_objects_topic: "/perception/object_recognition/objects" + distance_threshold: 1.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index 2c8c989f..483d4cd0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -100,5 +100,16 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml index 9501d94e..0dea8897 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml @@ -1,7 +1,10 @@ + + + \ No newline at end of file From 9f75c9492ac02db832357b9885e2a86b94772918 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 30 Aug 2024 02:41:56 +0900 Subject: [PATCH 02/12] add costmap to rviz Signed-off-by: Autumn60 --- .../config/autoware.rviz | 486 ++++++++++++++---- 1 file changed, 399 insertions(+), 87 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index 1539328f..2d248d76 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -3,9 +3,11 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Planning1 Splitter Ratio: 0.557669460773468 - Tree Height: 397 + Tree Height: 242 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -13,7 +15,8 @@ Panels: Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views - Expanded: ~ + Expanded: + - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel @@ -39,7 +42,8 @@ Visualization Manager: Show Arrows: true Show Axes: true Show Names: true - Tree: {} + Tree: + {} Update Interval: 0 Value: false - Alpha: 0.5 @@ -64,9 +68,12 @@ Visualization Manager: Displays: - Class: rviz_plugins/SteeringAngle Enabled: true + Left: 85 + Length: 171 Name: SteeringAngle Scale: 17 Text Color: 25; 255; 240 + Top: 85 Topic: Depth: 5 Durability Policy: Volatile @@ -74,12 +81,15 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/steering_status Value: true + Value Scale: 0.14999249577522278 Value height offset: 0 - Class: rviz_plugins/ConsoleMeter Enabled: true + Left: 341 + Length: 171 Name: ConsoleMeter - Scale: 3 Text Color: 25; 255; 240 + Top: 85 Topic: Depth: 5 Durability Policy: Volatile @@ -87,8 +97,9 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/velocity_status Value: true + Value Scale: 0.14999249577522278 Value height offset: 0 - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Class: rviz_plugins/VelocityHistory Color Border Vel Max: 3 Constant Color: @@ -157,8 +168,8 @@ Visualization Manager: Max Alpha: 0.5 Max Range: 100 Max Wave Alpha: 0.5 - Min Alpha: 0.01 - Min Wave Alpha: 0.01 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 Name: PolarGridDisplay Reference Frame: base_link Value: true @@ -166,18 +177,20 @@ Visualization Manager: Wave Velocity: 40 - Class: rviz_plugins/MaxVelocity Enabled: true + Left: 397 + Length: 64 Name: MaxVelocity Text Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/current_max_velocity + Top: 187 + Topic: /planning/scenario_planning/current_max_velocity Value: true + Value Scale: 0.25 - Class: rviz_plugins/TurnSignal Enabled: true + Height: 171 + Left: 131 Name: TurnSignal + Top: 233 Topic: Depth: 5 Durability Policy: Volatile @@ -185,6 +198,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/turn_indicators_status Value: true + Width: 341 Enabled: true Name: Vehicle Enabled: true @@ -213,11 +227,12 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 1 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Transient Local + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /map/pointcloud_map @@ -248,7 +263,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.4 + - Alpha: 0.4000000059604645 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 5 @@ -270,18 +285,19 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 1 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /sensing/lidar/concatenated/pointcloud Use Fixed Frame: false Use rainbow: true Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 Enabled: false @@ -289,6 +305,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /sensing/lidar/crop_box_filter/crop_box_polygon @@ -327,6 +344,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /sensing/gnss/pose_with_covariance @@ -339,7 +357,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance @@ -369,11 +387,12 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/initial_pose_with_covariance - Value: true - - Alpha: 0.999 + Value: false + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance @@ -403,27 +422,29 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/pose_with_covariance - Value: true + Value: false - Buffer Size: 200 Class: rviz_plugins::PoseHistory Enabled: false Line: + Alpha: 0.9990000128746033 Color: 170; 255; 127 Value: true Width: 0.10000000149011612 - Alpha: 0.999 Name: PoseHistory Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/pose - Value: true - - Alpha: 0.999 + Value: false + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -450,13 +471,14 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /localization/util/downsample/pointcloud Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -483,6 +505,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/points_aligned @@ -492,7 +515,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MonteCarloInitialPose - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -508,14 +532,15 @@ Visualization Manager: Class: rviz_plugins::PoseHistory Enabled: true Line: + Alpha: 0.9990000128746033 Color: 0; 255; 255 Value: true Width: 0.10000000149011612 - Alpha: 0.999 Name: PoseHistory Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_twist_fusion_filter/pose @@ -528,7 +553,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 15 @@ -550,11 +575,12 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 3 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /perception/obstacle_segmentation/pointcloud @@ -568,16 +594,16 @@ Visualization Manager: - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: true Display Predicted Path Confidence: true @@ -585,21 +611,23 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: DetectedObjects - Namespaces: {} + Namespaces: + {} PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -608,24 +636,25 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/detection/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal Enabled: false Name: Detection - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: true Display Predicted Path Confidence: true @@ -633,21 +662,23 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: TrackedObjects - Namespaces: {} + Namespaces: + {} PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -656,24 +687,25 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/tracking/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal Enabled: false Name: Tracking - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: false Display Predicted Path Confidence: true @@ -681,21 +713,28 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: PredictedObjects - Namespaces: {} + Namespaces: + acceleration: true + label: true + shape: true + twist: true + uuid: true + velocity: true PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -704,20 +743,22 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Maneuver - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort Value: /perception/object_recognition/prediction/maneuver - Value: true + Value: false Enabled: true Name: Prediction Enabled: true @@ -741,7 +782,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MapBasedDetectionResult - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -798,7 +840,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/mission_planning/route_marker Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.30000001192092896 Class: rviz_default_plugins/Pose @@ -813,6 +855,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/echo_back_goal_pose @@ -828,18 +871,36 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/trajectory Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Scale: 0.30000001192092896 @@ -855,16 +916,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/path Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.4000000059604645 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.4000000059604645 Color: 0; 0; 0 @@ -878,16 +962,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/lane_change Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -901,16 +1008,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/ext_request_lane_change_right Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -924,16 +1054,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/ext_request_lane_change_left Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -947,16 +1100,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/avoidance Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -970,16 +1146,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/pull_out Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -993,16 +1192,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/pull_over Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -1018,7 +1240,6 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound @@ -1414,18 +1635,36 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/trajectory Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Scale: 0.30000001192092896 @@ -1608,6 +1847,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid @@ -1619,7 +1859,7 @@ Visualization Manager: Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates Use Timestamp: false Value: false - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Arrow Length: 0.30000001192092896 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -1635,11 +1875,12 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Arrow Length: 0.5 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -1655,6 +1896,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array @@ -1684,7 +1926,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: PlanningErrorMarker - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1694,6 +1937,31 @@ Visualization Manager: Value: true Enabled: false Name: Diagnostic + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/costmap/costmap_generator/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/costmap/costmap_generator/costmap_updates + Use Timestamp: false + Value: true + Enabled: true + Name: Costmap Enabled: true Name: Planning - Class: rviz_common/Group @@ -1705,16 +1973,34 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /control/trajectory_follower/lateral/predicted_trajectory Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 1 Color: 255; 255; 255 Constant Color: true Value: true Width: 0.05000000074505806 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 1 Color: 0; 0; 0 @@ -1724,7 +2010,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/MPC - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1735,7 +2022,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/PurePursuit - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1797,15 +2085,15 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Theta std deviation: 0.2617993950843811 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 @@ -1813,26 +2101,47 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/goal - - Class: rviz_plugins/PedestrianInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 0 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 1 Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/CarInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 3 + W vehicle width: 1.7999999523162842 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 0 Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/BusInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 0 + W vehicle width: 2.5 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 0 @@ -1845,6 +2154,9 @@ Visualization Manager: Z position: 0 - Class: rviz_plugins/DeleteAllObjectsTool Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: @@ -1858,7 +2170,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 10 + Scale: 10.328941345214844 Target Frame: viewer Value: TopDownOrtho (rviz_default_plugins) X: 0 @@ -1908,12 +2220,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1016 + Height: 1376 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -1922,6 +2234,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 + Width: 3370 X: 70 Y: 27 From d73cca455ccb5218db638ebb1f1dd3b29362fa3b Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 06:20:07 +0900 Subject: [PATCH 03/12] MPPI operation check --- aichallenge/pkill_all.sh | 0 .../launch/components/planning.launch.xml | 12 + .../config/mppi_controller.param.yaml | 26 + .../mppi_controller/mppi_controller/MPPI.py | 463 ++++++++++++++++++ .../mppi_controller/__init__.py | 0 .../__pycache__/MPPI.cpython-310.pyc | Bin 0 -> 10093 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 165 bytes .../cost_map_tensor.cpython-310.pyc | Bin 0 -> 1831 bytes .../mppi_controller.cpython-310.pyc | Bin 0 -> 7808 bytes .../mppi_controller_node.cpython-310.pyc | Bin 0 -> 6746 bytes .../mppi_controller/cost_map_tensor.py | 49 ++ .../mppi_controller/mppi_controller.py | 244 +++++++++ .../mppi_controller/mppi_controller_node.py | 250 ++++++++++ .../mppi_controller/package.xml | 16 + .../mppi_controller/resource/mppi_controller | 0 .../mppi_controller/setup.cfg | 4 + .../mppi_controller/setup.py | 33 ++ .../config/autoware.rviz | 75 ++- .../launch/aichallenge_system.launch.xml | 2 +- 19 files changed, 1160 insertions(+), 14 deletions(-) mode change 100644 => 100755 aichallenge/pkill_all.sh create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller_node.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py diff --git a/aichallenge/pkill_all.sh b/aichallenge/pkill_all.sh old mode 100644 new mode 100755 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index 483d4cd0..05e295a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -111,5 +111,17 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml new file mode 100644 index 00000000..96d2990a --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + # mppi + horizon : 25 + num_samples : 3000 + u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad) + u_max : [2.0, 0.25] + sigmas : [0.5, 0.1] # sample range + lambda : 1.0 + auto_lambda : false + # reference path + DL : 0.1 + loolahead_distance : 3.0 + reference_path_interval : 0.85 + # cost weights + Qc : 2.0 + Ql : 3.0 + Qv : 2.0 + Qo : 10000.0 + Qin : 0.01 + Qdin : 0.5 + # model param + delta_t : 0.1 + vehicle_L : 1.0 + V_MAX : 8.0 + diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py new file mode 100644 index 00000000..18ca6044 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -0,0 +1,463 @@ +""" +Kohei Honda, 2023. +""" + +from __future__ import annotations + +from typing import Callable, Tuple, Dict +import torch +import torch.nn as nn +from torch.distributions.multivariate_normal import MultivariateNormal + + +class MPPI(nn.Module): + """ + Model Predictive Path Integral Control, + J. Williams et al., T-RO, 2017. + """ + + def __init__( + self, + horizon: int, + num_samples: int, + dim_state: int, + dim_control: int, + dynamics: Callable[[torch.Tensor, torch.Tensor], torch.Tensor], + cost_func: Callable[[torch.Tensor, torch.Tensor, Dict], torch.Tensor], + u_min: torch.Tensor, + u_max: torch.Tensor, + sigmas: torch.Tensor, + lambda_: float, + auto_lambda: bool = False, + exploration: float = 0.0, + use_sg_filter: bool = False, + sg_window_size: int = 5, + sg_poly_order: int = 3, + device=torch.device("cuda"), + dtype=torch.float32, + seed: int = 42, + ) -> None: + """ + :param horizon: Predictive horizon length. + :param predictive_interval: Predictive interval (seconds). + :param delta: predictive horizon step size (seconds). + :param num_samples: Number of samples. + :param dim_state: Dimension of state. + :param dim_control: Dimension of control. + :param dynamics: Dynamics model. + :param cost_func: Cost function. + :param u_min: Minimum control. + :param u_max: Maximum control. + :param sigmas: Noise standard deviation for each control dimension. + :param lambda_: temperature parameter. + :param exploration: Exploration rate when sampling. + :param use_sg_filter: Use Savitzky-Golay filter. + :param sg_window_size: Window size for Savitzky-Golay filter. larger is smoother. Must be odd. + :param sg_poly_order: Polynomial order for Savitzky-Golay filter. Smaller is smoother. + :param device: Device to run the solver. + :param dtype: Data type to run the solver. + :param seed: Seed for torch. + """ + + super().__init__() + + # torch seed + torch.manual_seed(seed) + + # check dimensions + assert u_min.shape == (dim_control,) + assert u_max.shape == (dim_control,) + assert sigmas.shape == (dim_control,) + # assert num_samples % batch_size == 0 and num_samples >= batch_size + + # device and dtype + if torch.cuda.is_available() and device == torch.device("cuda"): + self._device = torch.device("cuda") + else: + self._device = torch.device("cpu") + print(f"Device: {self._device}") + self._dtype = dtype + + # set parameters + self._horizon = horizon + self._num_samples = num_samples + self._dim_state = dim_state + self._dim_control = dim_control + self._dynamics = dynamics + self._cost_func = cost_func + self._u_min = u_min.clone().detach().to(self._device, self._dtype) + self._u_max = u_max.clone().detach().to(self._device, self._dtype) + self._sigmas = sigmas.clone().detach().to(self._device, self._dtype) + self._lambda = lambda_ + self._exploration = exploration + self._use_sg_filter = use_sg_filter + self._sg_window_size = sg_window_size + self._sg_poly_order = sg_poly_order + + # noise distribution + self._covariance = torch.zeros( + self._horizon, + self._dim_control, + self._dim_control, + device=self._device, + dtype=self._dtype, + ) + self._covariance[:, :, :] = torch.diag(sigmas**2).to(self._device, self._dtype) + self._inv_covariance = torch.zeros_like( + self._covariance, device=self._device, dtype=self._dtype + ) + for t in range(1, self._horizon): + self._inv_covariance[t] = torch.inverse(self._covariance[t]) + + zero_mean = torch.zeros(dim_control, device=self._device, dtype=self._dtype) + self._noise_distribution = MultivariateNormal( + loc=zero_mean, covariance_matrix=self._covariance + ) + + self._sample_shape = torch.Size([self._num_samples]) + + # sampling with reparameting trick + self._action_noises = self._noise_distribution.rsample( + sample_shape=self._sample_shape + ) + + zero_mean_seq = torch.zeros( + self._horizon, self._dim_control, device=self._device, dtype=self._dtype + ) + self._perturbed_action_seqs = torch.clamp( + zero_mean_seq + self._action_noises, self._u_min, self._u_max + ) + + self._previous_action_seq = zero_mean_seq + + # init satitzky-golay filter + self._coeffs = self._savitzky_golay_coeffs( + self._sg_window_size, self._sg_poly_order + ) + self._actions_history_for_sg = torch.zeros( + self._horizon - 1, self._dim_control, device=self._device, dtype=self._dtype + ) # previous inputted actions for sg filter + + # inner variables + self._state_seq_batch = torch.zeros( + self._num_samples, + self._horizon + 1, + self._dim_state, + device=self._device, + dtype=self._dtype, + ) + self._weights = torch.zeros( + self._num_samples, device=self._device, dtype=self._dtype + ) + self._optimal_state_seq = torch.zeros( + self._horizon + 1, self._dim_state, device=self._device, dtype=self._dtype + ) + + # auto lambda tuning + self._auto_lambda = auto_lambda + if auto_lambda: + self.log_tempature = torch.nn.Parameter( + torch.log( + torch.tensor([self._lambda], device=self._device, dtype=self._dtype) + ) + ) + self.optimizer = torch.optim.Adam([self.log_tempature], lr=1e-2) + + def reset(self): + """ + Reset the previous action sequence. + """ + self._previous_action_seq = torch.zeros( + self._horizon, self._dim_control, device=self._device, dtype=self._dtype + ) + self._actions_history_for_sg = torch.zeros( + self._horizon - 1, self._dim_control, device=self._device, dtype=self._dtype + ) # previous inputted actions for sg filter + + def forward( + self, state: torch.Tensor, info: Dict = {} + ) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Solve the optimal control problem. + Args: + state (torch.Tensor): Current state. + Returns: + Tuple[torch.Tensor, torch.Tensor]: Tuple of predictive control and state sequence. + """ + assert state.shape == (self._dim_state,) + + if not torch.is_tensor(state): + state = torch.tensor(state, device=self._device, dtype=self._dtype) + else: + if state.device != self._device or state.dtype != self._dtype: + state = state.to(self._device, self._dtype) + + mean_action_seq = self._previous_action_seq.clone().detach() + + # random sampling with reparametrization trick + self._action_noises = self._noise_distribution.rsample( + sample_shape=self._sample_shape + ) + + # noise injection with exploration + threshold = int(self._num_samples * (1 - self._exploration)) + inherited_samples = mean_action_seq + self._action_noises[:threshold] + self._perturbed_action_seqs = torch.cat( + [inherited_samples, self._action_noises[threshold:]] + ) + + # clamp actions + self._perturbed_action_seqs = torch.clamp( + self._perturbed_action_seqs, self._u_min, self._u_max + ) + + # rollout samples in parallel + self._state_seq_batch[:, 0, :] = state.repeat(self._num_samples, 1) + + for t in range(self._horizon): + self._state_seq_batch[:, t + 1, :] = self._dynamics( + self._state_seq_batch[:, t, :], + self._perturbed_action_seqs[:, t, :], + ) + + # compute sample costs + costs = torch.zeros( + self._num_samples, self._horizon, device=self._device, dtype=self._dtype + ) + action_costs = torch.zeros( + self._num_samples, self._horizon, device=self._device, dtype=self._dtype + ) + initial_state = self._state_seq_batch[:, 0, :] + for t in range(self._horizon): + prev_index = t - 1 if t > 0 else 0 + prev_state = self._state_seq_batch[:, prev_index, :] + prev_action = self._perturbed_action_seqs[:, prev_index, :] + # info update + info["prev_state"] = prev_state + info["prev_action"] = prev_action + info["initial_state"] = initial_state + info["t"] = t + costs[:, t] = self._cost_func( + self._state_seq_batch[:, t, :], + self._perturbed_action_seqs[:, t, :], + info, + ) + action_costs[:, t] = ( + mean_action_seq[t] + @ self._inv_covariance[t] + @ self._perturbed_action_seqs[:, t].T + ) + + prev_state = self._state_seq_batch[:, -2, :] + info["prev_state"] = prev_state + zero_action = torch.zeros( + self._num_samples, + self._dim_control, + device=self._device, + dtype=self._dtype, + ) + terminal_costs = self._cost_func( + self._state_seq_batch[:, -1, :], zero_action, info + ) + + # In the original paper, the action cost is added to consider KL div. penalty, + # but it is easier to tune without it + costs = ( + torch.sum(costs, dim=1) + + terminal_costs + # + torch.sum(self._lambda * action_costs, dim=1) + ) + + # calculate weights + self._weights = torch.softmax(-costs / self._lambda, dim=0) + + # find optimal control by weighted average + optimal_action_seq = torch.sum( + self._weights.view(self._num_samples, 1, 1) * self._perturbed_action_seqs, + dim=0, + ) + + mean_action_seq = optimal_action_seq + + # auto-tune temperature parameter + # Refer E step of MPO algorithm: + # https://arxiv.org/pdf/1806.06920 + if self._auto_lambda: + for _ in range(1): + self.optimizer.zero_grad() + tempature = torch.nn.functional.softplus(self.log_tempature) + cost_logsumexp = torch.logsumexp(-costs / tempature, dim=0) + epsilon = 0.1 # tolerance hyperparameter for KL divergence + loss = tempature * (epsilon + torch.mean(cost_logsumexp)) + loss.backward() + self.optimizer.step() + self._lambda = torch.exp(self.log_tempature).item() + + # calculate new covariance + # https://arxiv.org/pdf/2104.00241 + # covariance = torch.sum( + # self._weights.view(self._num_samples, 1, 1) + # * (self._perturbed_action_seqs - optimal_action_seq) ** 2, + # dim=0, + # ) # T x dim_control + + # small_cov = 1e-6 * torch.eye( + # self._dim_control, device=self._device, dtype=self._dtype + # ) + # self._covariance = torch.diag_embed(covariance) + small_cov + + # for t in range(1, self._horizon): + # self._inv_covariance[t] = torch.inverse(self._covariance[t]) + # zero_mean = torch.zeros(self._dim_control, device=self._device, dtype=self._dtype) + # self._noise_distribution = MultivariateNormal( + # loc=zero_mean, covariance_matrix=self._covariance + # ) + + if self._use_sg_filter: + # apply savitzky-golay filter to N-1 previous action history + N optimal action seq + prolonged_action_seq = torch.cat( + [ + self._actions_history_for_sg, + optimal_action_seq, + ], + dim=0, + ) + + # appply sg filter for each control dimension + filtered_action_seq = torch.zeros_like( + prolonged_action_seq, device=self._device, dtype=self._dtype + ) + for i in range(self._dim_control): + filtered_action_seq[:, i] = self._apply_savitzky_golay( + prolonged_action_seq[:, i], self._coeffs + ) + + # use only N step optimal action seq + optimal_action_seq = filtered_action_seq[-self._horizon :] + + # predivtive state seq + expanded_optimal_action_seq = optimal_action_seq.repeat(1, 1, 1) + optimal_state_seq = self._states_prediction(state, expanded_optimal_action_seq) + + # update previous actions + self._previous_action_seq = optimal_action_seq + + # stuck previous actions for sg filter + optimal_action = optimal_action_seq[0] + self._actions_history_for_sg = torch.cat( + [self._actions_history_for_sg[1:], optimal_action.view(1, -1)] + ) + + return optimal_action_seq, optimal_state_seq + + def get_top_samples(self, num_samples: int) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Get top samples. + Args: + num_samples (int): Number of state samples to get. + Returns: + Tuple[torch.Tensor, torch.Tensor]: Tuple of top samples and their weights. + """ + assert num_samples <= self._num_samples + + # large weights are better + top_indices = torch.topk(self._weights, num_samples).indices + + top_samples = self._state_seq_batch[top_indices] + top_weights = self._weights[top_indices] + + top_samples = top_samples[torch.argsort(top_weights, descending=True)] + top_weights = top_weights[torch.argsort(top_weights, descending=True)] + + return top_samples, top_weights + + def get_samples_from_posterior( + self, optimal_solution: torch.Tensor, state: torch.Tensor, num_samples: int + ) -> Tuple[torch.Tensor]: + assert num_samples <= self._num_samples + + # posterior distribution of MPPI + # covaraince is the same as noise distribution + posterior_distribution = MultivariateNormal( + loc=optimal_solution, covariance_matrix=self._covariance + ) + + # sampling control input sequence from posterior + samples = posterior_distribution.sample(sample_shape=torch.Size([num_samples])) + + # get state sequence from sampled control input sequence + predictive_states = self._states_prediction(state, samples) + + return samples, predictive_states + + def _states_prediction( + self, state: torch.Tensor, action_seqs: torch.Tensor + ) -> torch.Tensor: + state_seqs = torch.zeros( + action_seqs.shape[0], + self._horizon + 1, + self._dim_state, + device=self._device, + dtype=self._dtype, + ) + state_seqs[:, 0, :] = state + # expanded_optimal_action_seq = action_seq.repeat(1, 1, 1) + for t in range(self._horizon): + state_seqs[:, t + 1, :] = self._dynamics( + state_seqs[:, t, :], action_seqs[:, t, :] + ) + return state_seqs + + def _savitzky_golay_coeffs(self, window_size: int, poly_order: int) -> torch.Tensor: + """ + Compute the Savitzky-Golay filter coefficients using PyTorch. + + Parameters: + - window_size: The size of the window (must be odd). + - poly_order: The order of the polynomial to fit. + + Returns: + - coeffs: The filter coefficients as a PyTorch tensor. + """ + # Ensure the window size is odd and greater than the polynomial order + if window_size % 2 == 0 or window_size <= poly_order: + raise ValueError("window_size must be odd and greater than poly_order.") + + # Generate the Vandermonde matrix of powers for the polynomial fit + half_window = (window_size - 1) // 2 + indices = torch.arange( + -half_window, half_window + 1, dtype=self._dtype, device=self._device + ) + A = torch.vander(indices, N=poly_order + 1, increasing=True) + + # Compute the pseudo-inverse of the matrix + pseudo_inverse = torch.linalg.pinv(A) + + # The filter coefficients are given by the first row of the pseudo-inverse + coeffs = pseudo_inverse[0] + + return coeffs + + def _apply_savitzky_golay( + self, y: torch.Tensor, coeffs: torch.Tensor + ) -> torch.Tensor: + """ + Apply the Savitzky-Golay filter to a 1D signal using the provided coefficients. + + Parameters: + - y: The input signal as a PyTorch tensor. + - coeffs: The filter coefficients as a PyTorch tensor. + + Returns: + - y_filtered: The filtered signal. + """ + # Pad the signal at both ends to handle the borders + pad_size = len(coeffs) // 2 + y_padded = torch.cat([y[:pad_size].flip(0), y, y[-pad_size:].flip(0)]) + + # Apply convolution + y_filtered = torch.conv1d( + y_padded.view(1, 1, -1), coeffs.view(1, 1, -1), padding="valid" + ) + + return y_filtered.view(-1) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..624e444edbe033ca6540a9d6462d8a7a15a6e5de GIT binary patch literal 10093 zcmbtaON<*=cCA0Z&Hjk$pZqDwmMz*+%aZ?XjctwXamE8_6ic2-+Hq=1d_{J#i&fOC zVoT~NW`an@APEF`6(AV|VIx2gXOZt}Hpw~&5Cqu_@Q_U~D=o6iN>1E4_Z5p|cT0+q zu))js|J--qz31N7(acO)!Dr^)zqHnJndy&#%)5%B#v4d5WkX>q(*m`tZD=Z` z^+4|$8%EdMFuT@ zRi#nfsh7VOZF;`_D^bYY)Asq7&cD1?zE5jusG{PAVU)OuABAy47sdD7AaJh-o-nWV zd-yTl_nXOOG+emU4-$Xd<-VJEmm}VFgC>nJ{V8S{HNK04r);R$m&Vi$ooS4|1~WZl zXfkV8^^6Wy+bXiReyX%oX0zjWXRx;u?3ugDM$xCT3+xnY+%-2!>|5*w_TpV*qs-oB zYwRpi74{B0$Ic^FW$&_A*=tD6u#4#gwgCX_x=l%ENn*H--$w@`(Yv~VZZCdZWnBgMTz;yfbpKF zP_miLEsE@R=yv^P{NR=RC_eo(iFk8!?V1qx4UjE3W3|b&Rj>Btsru-KewMmPF5(4>roVlig&9Q zL|lrOnCZu!6Stj~A0!?ZH6(BPA&YK0vA^Sq8RUCWaNCJEL$wim9upS#l0FZGaXAXT z=8>G;_+7-egfDom#x>AHxuXo#T}4$AZK$UUyQ-?Bg^rOLEp+A(^I8W{F(B{+8vs49;M}! z>ME%QT1`>!%%lcrEj2-BQwwx1Eez*(mEi)uM`#3m%e&O0o)#Zbb2KeIqGmBIqvuju z1zk>OKv&Wl=xRC(dJOw|0^?WkMXCJC`IvE!R`eux^i*16<`HFA-M^a7lh7*G=h`%F z=@-D`&eIs_NqqB>XrFd+oE1=3xude-7zqJI{kY$4d98{Oe) zFU|SVWb8xzNG{H$I2q@BTKPidpQG)|!&k6=NL}YO%&^oZzKzG*&A9P8YTxL*iJI5j zDl4;!p2uEGvL^Htp!VPd43BGtFvbGh5KM+#|uS_zKI?7$1#$!4b6rn>aT>e7K0 z^Z2$K?CYKv+NWX>iSl6N2y3m<0ZL&Qn+lRIu>D~kFN2P~*j1)AQk5YD*?8>2rV@SFtFY>EI09@6>-~ho16JW zHtbKvrb8!;bJGj6Q}e_2p}zZ! z!@B(m)_&F9_LH5h+h;zE0{6C^*^NUT_nVP*SZ%T?nNZPIA3lm5xV#NP@?$&hMp3ef z?bw(4IHv2K9WnN>X(rYQ7XX$jjJiHxgS1zYtB;uKDzH*;V6KA~la8n9L8jy{Y`o3; zp^atPaTIJngx4&&-J|wy;@Xt_23^RauG?2ZsR=7v znK0#q_cb)Sd|w4cogPF@G2e_NDh!(*;6cLuTVJZ8n%zz(-gJ9j`^WXa{P5p*?!E&U zsRg|8mN4T!#7Pt#s1!eO03qaJM5XJ7eK&BV?KfrTY*|%5cHC{(m)Mj)M=LEj`O12n z;Faab6=KjFbyb}y)gj8x1aehndUVl5$;m+%8jWwQFq;8D2QeVQZ4p`$VGem$vtK}= z0-#HVg>u|eb*3(Xs5$$@TFesunFc*IDwxQXFn2s2#iD{`%8v zhuhOlOzwtlPZUt>@z~=QG3baBk_y09W1PzMK3#rcT$NaBghR9Q5e1$d7uM@a6_xdj zi3Px`V$s2A;qG7eSl%G^Zp5PQK&5Z{Q6Gns^+PdQ$D@nuwCUnvpldVYH|iN%5w^%5U9Y0^eiooUFag`p^2Au~tH ziG;H)tVDulF@ualN=&LSValxA z_s_b1bCYxj4)^TMh;PL`x9Od|-uDA`w%hCZ;HY$2pj{5-$?sX~-R9>oEiF49f|Oq@ z>vQ^?I;SohHMOeEsg`Q#_>TToRrC>=sZz=|E9EFgKMV>0y~Au21cW12*B2kQ$=2Sze>ULC8ykG&Xf z9x2>hrtM6oZJbQsgLa#k1bz~;^9IP}Mv=Y|PkG-Pn*kC#C~f3rAhOrw`PyjvfT=%`H7Xi8Mh{YfI;6 zQgv9$+wN)u>Q^3C;Mi1ml>xDSM#AY?rb*a7m%vHtEJ(-c$g)yb?x+KU+@#JNgS+;* zwxTR40}~LqP7YINE;X1eZD&pwf%`?26TDx-cUc1aqgcfXau}BYeP;(jtbZ0)BsM{kldG1yVT0ttq@m^X1VFo=}}RxFDZEsI5fWnjy;8b3p4}1Ip2&y z;gL1OzeB5BQ3j=Cvd$3ew3xYN@PDH@mz7Xu<^axK-ZL%dt3Xe3@bZ;UmsjCnz=gY@ zz(Lel0ly{u7V&H0w}fA1NbcM>=0&Q3=Y^a{Ee#{+sU6q9=33zo#QAgPWQO+ zuag2epN)lPAaoX81J;do82msKXc;rwE4gBtoigB9wm3dg7hd7^W8HZ4?W zWhaj*kPE7TTPa(@sT`7P3F(3SkSrdpkJ=1o(^)CSr{JFbB00u%DM4$4IkajFU&ulZ zofkW&hi6c-wyO^2G3Hr0*L*(HV1dCMMtFc+;+H5qu>{E<)YJO#9E~8yI!~!D)Ej?- zHZNym4PV(sopTboSHI9WdP`^awRE;kvW-2Ev)g%HI=eZC^ueN(p%ZeYiv!$};Tvf# z8B25?81GF;^y275P}sQ1ixsCUEe&e7w)4xe+PzBXR;t~Rc^`WvJsu$r*_~+J zi@Y7Lk2{iwDpjI=YC1^OfV0)-+zXSecGCT08A&=Y!vCs5*KOH^9LOp6b!2vJ_*SDu zJjzI%fyT`&1tl+MM5F=y7Nw*;e4A32L4bSigB98m{kU7e`^^SZ2X* z&?uZ5&C-O^*te8y!d!1g0Tc6n2&WR$vOMT34;7di!GKs)M~GSG_%@A}$p+k4?-oyJ zjy{!EBt{%bNe##rfmw)2Hp)T{ibtbC429~5(gf<(q(d{|QNioQJ{%ii1W^nKk86fp zdmpkFM>1b~f1&D&V_+p@9_0_2cRnwZj3YoRYI#w%yo1=M04PHT^*7jO{Ldf@<+8S@ zE(4a@8N2`$YkAms_OrV(IVRG<3VuqKdmS+A*ebs2v}tq8bDfRkEj>fC=@ zRuKSmLDRmT+gnCD2egK2{g+{_s>WWmprM7fw_GrE>SKLvm_^K2#>{oJU%+@ZqKk%R z0QhQOYliieQ7U6aHNf8mwZ1oJ)^w~;FCxE;@}jX*mH2J6$tJnZ`G)`;eI*$CO(Z1P zQnu8%Ac2@V%uubm3m`V2w3(7RCc~u56aZ7Ab}T?6yc^xi>Wl*=1gZ?Fa`l~Gj5X(n zgmWW=-Lj{MiH1C^V5j_N`xKmPXxFI#90AgN6be7Iy<{@j|6AQoEm7)OKy#nlBX@8z z&o$`AGV|i57c#_^8ih=ANu_9DjIB(;z%|A#_F}25L;=Acyrm=!GrQ5`>WKS!OizK^dOKt{v(*YV1PI$24B8IH#Ajvbvx_ZnT}cl!XJ0#awh)(!>0E{pM>Pt=7a0~d_ z$^_&xe*t50vR63|$=TxH1HqGzl$FfY`Iw4-KWisFnlGt9Dn-f1#U2zKG#`BWeW8a! z&d&Kp0Qbz5ie~J4C7=F*hO;l5z;pG-C2HrX8SGpMFAY}mY zB-pPEi+L;D)KM#ps%7cvGJvtscqqk;d22Wq&!o~@y(}SwL=(~jZIq^bPl*Sl{gt3# zV*jOKCYziKyF97CMu8}gHFYd)(}81)>n%K(!;0JC*aEOEJ@xU zHkhI^9qTGj;#~XqPp`%#YEqzkNTe!L7Zs-A)s9}$v@Mm_5`|zlUhwexy{s^G6}k}G z4m<;(2jr9|MA{K=%=e4|X+jfkZg_JBC-Kq@2i7F*I4nT$Rq0fb&Mc$_0Mep+8oL}T z$oHaduMgEm!GMQ`bb(P3(ej%s54cLLefyiEg#Bwyp}`S_)NBkI2$K3_xrcy z#d!A@rehTL##c}7AsU6u<3ztf}%>fV*`c5|8aBB5yQivN`78oOhdaa=e z^|C1YVG~`5qm7~{|G*9U-UpmVJVUt_qIUjMf@OUiqI=iF#4gda>7M5d~8zYAUQOPfbVJ)YHj@8`hCS8W{O{3mpU9KAlOC5MPfJap67E3kGqHy8hptX_^1z1Vc|q(+ z-M}l;CS`4uOF@U0$3IoTe`Y2;S8bBc1wS=W2CISn5Yjf- zI}&x0RKUh zMDOhX9*Vae8VUdGP@XL9pbY*VI{utwkYa?@+8P402-EJBjh)p@_8#WQG)`oJZ0XOD zY0f-58Pb+zGOjIC^HjS)ghIufBs~jrFH%azx@BsavNE+oDY6S$==%vu$@sS{lRXg&dz$Mz+%FdU<$qCc4iD`_Iv97XEd|!)sRAaquME4=7!AoEv!3 z8r2jWheb`t$)tr8pD4)xo_YDa4AgV7e|$*f&x!mM5yD!Wv=ApT5Hq7Uf~FS)7%8L* zNdihmL`b*LPGZ7zMa59Hc{nZdk4!k6qW@5SQmkv$>YnnG^11TS@{#hw!c($eqsIRU zy=W7yK0Wwi4vh*dJTpQsa~&B@2sAsQ^Hf*NVlZLIe?-~mv+Okdi`TkS|3HQZCi)Mu q?4ioS|7C$$a`vh1$UV_^MDaq#&bM&`GH`7uRih3^uZq7#{eJ+4y2-}? literal 0 HcmV?d00001 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bd4bef9f354ab13a6bfeb727b79715c2440c1371 GIT binary patch literal 165 zcmd1j<>g`k0^Z&WX(0MBh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6v5KQS{oBQYl@ zH7`9?zdXMvySN}RIaNQYG&3hfKewPDGd?*#ucRm+sG2 SKczG$)edAsF%ytrVE_QkFDWem literal 0 HcmV?d00001 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3f99c96c85732caae4e643e101ce192d77ce0711 GIT binary patch literal 1831 zcmZux&1)n@6tAlO%p}>(W+lX3g<=mf4IzOQFGG-p90b95aPcrAO>I|Arf2%g>K@I8 z&P8+e>@{=rUl9B!Jl55N?4RJG;(OIIlMY%?ufBfo)vNb@ugcxs0fF|@N54(>144es zMZY<)cmcis0E8rxW@L;b%V^HVjFL}?bfkMjr27LKJCc4uBJT}g$$3PL_i|BXD#2yH zZNcIN^!iH>ij1iwV8e0_DpY|AChT1zeHlzb8Op62Z@eW3 z7UQFwxN=+W+_>X`48Vz9vwK$7&1a%|sfxPP5&Hu!vrW4&7&lo+VZsDJaV{#em8dL> z>-0()ztrhPS{PreW}%Do#9wLRfO8M}8T9%&h?ce_CM^R=Tj$zo*vx62)@?mWe{^8i zg*l16)&uEF2F^KRP6-+CRYM;*_`nMFZiuf46L2 zSEwNx!EuUi<2_v<@30XwerMKrR_8nlG-gr*>51`d(2U=yncdFmxLazh5WNi|@{L=o zY;vcmLp<96ekOVkHr{@IBGP0kvP=~h>g2mp&+AGg>g3fT&E!d5RcV}*MWZ3dO8;lQ z&gJU<@WzB@5}OIn=J@;{V=%9TJXk1gynE|bIzHs48Y8QO_Qh}cO= zEG`9%tef{?r``wgz(1sWbk8}U&)741;JEDS$$gUb)uL@=Xd!YH$7T@6c_|ke&UfPY zn}x`_6YIDw78BV2;lt;~@1k!!yP=V-8t>Qdp}=D53G`PxAdtv4B0|Ev5bD5v>oWEa zbJ*Vlm;J?p!JkhKEfRuZ!{x*80DowA0h;KUwYS(=d7hrv&dxpnf)EIhkTkMvg&U(liDK-8#Yk33e#ns^%aT1p#wB9DVmWsmifWu1cz8-g3#s2Q%N_v+t!%rYhwH zx~JdW)7}5~fB!#eqv2}!{rH9dXn*n-n)aWRnf#f^d<7wEBjOroiMCxn*|v_no){f- z+hkN`Bv!}XwpH3pDjjFrQE8S`x2uu1@9x*Qg=f93bDLN0o2P8M@pX+myn0{b)l+@D z$=L^5%YBHsw5=AC)`$IG5+Px}`>k)ijc4P{G|S%&dLKsJEEUogQQjBbkWq)hkJ1Lh zn+VxuM1_`X`|J|dPS0g+MdepD%%pEU)X}1)ODj)BxF_{qj9J*>r<$OB`Tcv>f;ij@ zk|gTxM%V78;^V9rgweH~ew^@Yon9~Y!?c@=6fMM)@|(RQxe#=BlgRI;q7x+Xaa5&M z(i$%7qK-$1F!gI7_Z5WfzYs;*HscyLo<+u)$@OzQ_pP?U4Q}3N+cvh>*spL~#g)jR zCxy1YrnPOg(dKrQKhJ0DbYA22`_{J08&Qo{c(ZNt1-^LS+^!GCyf7G(Mzi@6zlgfV zRNZpa#JE)N@Mri6>KCeO+#9sHtomPx7Wq2DaQ;QLGM7KgpTq1IcoT>6f?Sx{*sn=5 z?DHVBum!^(rD<=}G5-?|yb6ucv|DsASsr0f+UsUO5 zRC-yZS5*2LmA<6XD=NLpk@MF$jdNMWS2zjQ>XvBYaHJ9T`qJwAowy4jvw3t7hmo{+ ze$>MQ<-wt}vv{`?WU{iCiugF~%38PI@w1=<(a#qyHbBYL`V=05K?KpA9tnsK99SCkf*gSZOq$_ck!0dPJdTa zXb!a*^*Hl`gCI_V9iYVpHIYBq65B8L#LgA6%`8t4pow+r(HOl#9%HZUEKLp~A)coO z9z`!u^f`)Nr0DY$eSxBvC<4Z7aw+T!5p{FF7vy_>+~v`sY>KEIp)ib8sdUslouJpU zq?tuYTU@37EsALAVu>OG$ug=TB^@85#kud(`(|$;vYE|R^(L$9He1)%**en=hdE5g zpE>%Y66b%`7pT5&)Y!ibz72m4 z5(KvjF@A*v-D98z5Mv)0un){YY;s078&6=SdIAf80hR9&vRDYz*5Xzl+nU?lK4m{- zdF8BfuANYSLJ-C)57_tg6Z6Cx)Yyex7^~Vz1ud%Lo4L!KGeU+6uP$rcJp~H!+NsVo z;E>hUAwl)v-*BH?Z}9t0kKxZxe)Ke)Hy9o(53 zdHz(q*=foqUU_h8siVQSHoR9ShaZpqZKd^RUPDoZ+mmmx;Z5f5-1LHwWX8*)yM4Oo zUMXv%2?zMI;a$hHK^R5}s`4ljcj!sm|Ae027rj?Dyc?5?p4tO49V>U@r=| z&tnXWg~&^z(;A8-uJJ*Tv<&fOTvYMb6#WgNt>X_yO$WP`?qCo=lm}VeP)DBxU6u1X z{Vex(Xxa2=rvjG;Tn3WjDcJBjQNEY*%@z}Xg~5-TWoaIFqMP0iPU7D|d=&N`Ti)$X zcBj>l7coHSV-0>Tg8eAO4j+k^X?fQW0RRS@Sfa%%ROWsZB>m{CLV&QDG`6f@Dfa@w z6`Dw^C*p1{Z4`EbTv{UScX`VZuhOWrF7YKAhD2TD(k&0s$5F_tIu(^PqAr)NTE9j%x!Q3WbTA^?fu@ivb+3&n{-Np|r~b znu1=^6{N$}T6VCLv`d_QqH~=H;28)PH_vqd=&;v0LO{;)~MW`Dz|fXrWc^e z^K;;oj1jbBtaIa}GSzx&X4TppC;~ju+R3fLI%OwTL9^I}bq<^2q^fe2LC!5ICpA^# z6b{uB>|fDN>V;7#y0@%d(oPxxdm?_W$~9@+g2D23ZbI?sCl`tfxPMOYv&Se9k?y4V0L^xSEaDyjk;iSN`|~(RJTBsc z$Qx3o|GVN)GU@FEc?i{{$jk^}RiU53?3sji=(IeT!b#Ff1FW9a7X#(@sPouokybw`*%VBhnAMGAYp+u0K|&ZKs^IaG;%;vL7sZ7Ht!@DY9waY!Y|J|r~x zlt}~J6-a9y#L+#)9;Fq6o%SX;Az6yx)@de<4D{207AeV}qJb}zwRo?k2@w4|nuqw0 zWJfS}Y11V;kd{((gsxC@Qs;*VM+gUC>t!RW8{P8{rFtC6D&;0B&`v~IKgnm{L$TEW z(`Cd!SDUMh{xv7(#$Jk1+kLTSd`b{di?F#HjHA0f)WU=Is} zh}(&YC!`w0OFL7fuCTa4*Z?z+K|V}ij=~T~kd8W)Ue%W26F5`!?J?~YwhY>~Q;>0iS?VXKqnHXJ<84LPjbO1_(!~MnT3j*0JA&2gi9p$kzn=X!{GG z(dKZDaqX9>-YIIAw4#B~#CjKs1)x(kcm;dR{G!gGYB*Yd)?;&a}mX_y-R6smTd_WNy zhSKPz_r}(ttoFMZ7;$tQDNZ2kVcO~S^GGRwSsB)ij7We^zoRTkOfqI^3Sc6dq)pK- z&7?=LmZL;gm8D5%EB>C!odX|i4kSi44x%ItzAU7GYp@JqpZRxXH;%*)|#0SU7E72lCafQjh$Nmd=miFK-< z=s8?f(D7Aha7Yd}ARP?M1RSHSBL&7oM&<+K#3h!2Ru%9Wn47>a_)bfyg&eE&8qf#~ z%6R}Y^2~uDTFa|o8Zan{g`CuLx18aWy`|lG5t0nU6Ovia$pozz))noPVJ%It4M=EF z{a6z_Sf|Q?`?y8pmUj26n7=xabBmfXRNdg;K{H1;bS%jbtqCi1Y%X{p`jgh=raS8V zv||4MVVu(W&6}e`C&-*BI))5o>6W+GpD}ppHh5RTASMiG6h(*Lv;j?|i?)Z>FnW1n zJp=1$I0l5B$EDFasF`a0);nVY10|d$eP!Pc=9(LP-tZiEADW^tnLTRfDGPOKwvpjE zZKICI7%`{80!AIm_Z$uOSPXoe`Hlf$wU39HxI;6{a=34gGr&hjwE=GTlxHzzNs()d zq?rITTY=i5?+69Sv<7L!H;{oes&62ZC6G#JS}<}*JJ}-zy8<3^W}v?bZEF<9nRSjf z(26F?loJ{bXYd$cfTmp0RfPd?J8^O?Tim{d_Fx`v4$I&a_RT6{NcHP*Eg1!z%x87T zKX8WIaCm=o_ic>Z07@{FU>=NGg^d6w7p)yUsfwB^^ftb{kRzN%fVEN`=cyN!VT~@3 z09FtFoJ;6^OQRHU#J>A>VUfkLRq7z(W#S!*ND+Oim##wPG{ojVS}4h_N#8;M zq)`l~YMx39r~aiIB0&>rK_!9YTFTut9tJ)Fd`5|+fzLAFhI!PH4!j!b(~O8|?3y^j zLpe{#Eeu)!H%lg6@+|jK%#g`?p5{R^%sE7B6{M3>LF9f6Cd^{l%%a^6xk~8^P`+1c zwTtSe_fjG(d@#Zc{gn*}q;g@aQGbKgWX~bGuCKtu`5c02EW^)9o={KUV2jX7&__4t z^wGS#W%CvkW{_mBZB9*5?mca=_yM9VMGe#msIzD_#3|L$MHc@+(LYi|mU-!lwxN&V zrWXa3+(#r0c<03rRSGXG-arb6Hdgp*i;tDB2$4WO$`yz{^c?dC<1_V&r z5MwBP@e^ub!g0=L_Dp?bId;Qvb<=*9a zH!u!-X$N?SdlVf~!4mna@nsy2-Wdbfv7KI*~Gr&cGIXLpId zrmAlWa?Y`&2oM}Teu=Q<`}{4|B#r$TUb-w4aW5CYM2Y%fsTT0KHbb)V^-ot>Tiz~GA{0D^=_isH&Lpbxu}IEqM>6v?Sblo&FOv#~NBY!4PQ zoZVUV%#y+ys2s?YeL+=HIpoVu0j?xfy!h;#>{R{%R}MM!DXAP%ELAE_=*WtqnEAb) z+1&vMIh93C_j|8jf4zSFJ{obOVQcvP^-F)h_2Csw`)5LmKLf}$e94bcaEc#dV>S}bVce;IA@!HXu-cox>*WT24jo0sMyzaAm zrS>v+c!M|Z>+Kc)ET6f}#FoEuPeUL5rutg553wuldP|qa^_cspt#^3b^HZ_YDoNwr zIPqoq-8kr{(!MeDQeX6gxZh&Zz9qcS@1(KVkuy_Rv0{|duXb+xqUZJduf_dT#L;VU z4;V*Nn_1yKbqI8PxS}YKGQs+|*i*oO>JV%QuDPk<>bRVL1%2iH}y5d8800|ZxoC*O<=5uTU|E6)oKbj*2J3ye1@*@@Oj>tz-xuMo+_;80>4N+0qzO*S>o(aKQIopu>40l zHx$JvP)tQJbBYO`s-jd2l)9qS3zQi}naL>?^qN(a*_>j5GOsA}1)c>(Stw9WDaxq= z<&2`7DNvRaWvM_}R+QxeWkpd|3Y1kvSuIe`E6Vu-Wld4mrWA!gHO3WoVT|!h-)4WM z@9Cgc548iEKfSB}x%M4MSLstNt@F$LnZweq<~G1nOPl;kj+3+qZ{@fHd^Vlq&+cmB zJZLx-mt->wp3i}{nA1o)v*~I69B5~9+ETh4t{j{_)c!yh!*uyzm4E(F?;8qQKg8QI(f4%hUV?@LWEG#<vG|k zIdJ>9aI&%;i(nM@A1>jR$y$Hda}%#Oi2Nitm;t3cbbCR+i$?nWFb9701J&StFmAXu zZn!GVBkrqU;S!6<=V9t6Bn>;aS_GH zf+^h!x?frw&BeoXFibac+v;|D{2^Jc(R|)8h+waMu9i8nxxYE=ZqSlPPSB^0w}Zal z^U|P`jFvOXbZhCv9gG@TTL(tF=MALM8FEjSI)mY@mLbh-IkK#VmnQe`1ReMop6(3% z?)T6A{)acd{})$VS7bRE4tyak7Xk}X*Ojv{w2>!Bio%13LSv)rrv-HW2&=c~%2uPX#)nge1Mh5$r-`*5B(8VI3nC8!5-lQCwxZZeUwTnI z3ry6BWasJw7HP$KtRU^_ys|PLyq<3|4SI^X?`BY0RR|)C#E-VbGZ^-PE~~DK^Gy=h zeV~1EYr_jV+t|L}@A?~eV{tnfcpZOZa~MQ?byHtp zqm?J)j`hjiDD{$Vha!*hk6`*YaKDvCu^izeLXxC>hLfoKHe**<3e}mwgR2-Z2SBOU6EfgotIn}fO=bUfS zuqVxV6%E75oEJ~n=kY6aB;KP-JC=0vWgryqDG#l~Loa+PSh>KvPDriBN8#=taU*4m&tR9@(#X2bf1biNapw4Ch1)s)jC%#6 zw+^1&Rf$ddIVe5kd+;1)_&mP#Lv7a%H{9oWP1P?Tr6L(wNRmh5 zyO#H+`URvunS@uvm&U7BBh}PMU%*H;8cBtX&Fc^8Mj{MSZi_tG5nWOy4MegA2^qgG zWcS`BTv|QULOrBQN_8nU4$2BPbD9bLqBhd4thSVnXwVTotagn*WbZRvU0rw>wq;9W zKpC`!GEmn~|3K`D5DnITWVGs@BIAJkj{B z8lN(FH^XEfn~z5uC*0XM_u}g>V_q*xUI8#3w^f!1HHia~K!R6QhXDlVarC5-XJsX1t&Q+xE6DAcK4$F0{=-24p-J@lbz*!K1zenZ^-%$fP$KA1+f z=x(R7d^h0fwzSb0M?)2@t2pI#VyY+PmB)H-4hI;>mowz6(|9n>OvPp5EhAS&BubuA zw5j+VD!xX=dsNUHmiPq}Dy5(lL!}hrD@0c=`K#23b$J}g$8Qs2ZBatx23-6iq0MPl zM%kPAB@kQnEYx#}p^j()DNH6&o}A70I<##!@!yx#Yybjlg&nFe1Uok~ zR#`ui0%ZxMN`H>4;ICm-_wXfs6v`SQLZ*n^m_+57U8-uPEoLQ{}$yz^7 z*QQtF5~jwZ=ps%wPE(|^e3->{IGWBIqm^ld{6yc;nfy?nXt~uYXE*E%7+tA;7Npa~ zfSp!p!ZEw?4Kz`Jnk=9&^*J0?u|&wXeuDe>IhVfGq-?)Ws_aVJb$c;~O9WhZ$s~^TUfzlj7(Y*LN6{J*x7AYHfW~m&KTTs5Yh()D4O+e&a5-KdM&YJHv zOU;s1vT6<&)Oti0D1snYrF7v6fE4kcd{jCg=I9xK@%raZojhj#I8j4lm&^(ri!)+~*l7xW(iO2`{P&2<~f zF1|}`5G1v_ZkBSbitiBBR`ac^!DtJz-uLcM0+Fnv$lf1jyMFfmKc$n_D?g(gj66^8 zUw7#xMm>b7OHO&jWSk1|)0*VDqpahXrwM13IO*JEHCx*IaUyDjV{=e&@&MvngtPu2 z^K9pPNB=*tehklEDW2dOo9EoTWoDnwRmn^I2|-f@SCA62^>U_$uh8{R=%}F`5c7hv VECh`sCxRWO{hrM&)7Fj0{|l9Y2e1GD literal 0 HcmV?d00001 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py new file mode 100644 index 00000000..fd977e60 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py @@ -0,0 +1,49 @@ +import torch +from typing import Tuple + +class CostMapTensor: + def __init__( + self, + cost_map: torch.Tensor, + cell_size: float, + origin: Tuple[float, float] = (0.0, 0.0), + device=torch.device("cuda"), + dtype=torch.float32, + ) -> None: + """ + cost map tensor for collision checking. + input: + cost_map (torch.Tensor): cost map tensor. + cell_size (float): size(m) of each cell in the cost map. + origin (Tuple[float, float]): origin of the cost map. (m, m) + device: device to run the computation. + dtype: data type of the tensor. + """ + self.cost_map: torch.Tensor = cost_map + self.cell_size: float = cell_size + origin: Tuple[float, float] = origin + self.origin_tensor = torch.tensor(origin, device=device, dtype=dtype) + self.device = device + self.dtype = dtype + + def compute_cost(self, x: torch.Tensor) -> torch.Tensor: + """ + Check collision in a batch of trajectories. + :param x: Tensor of shape (batch_size, traj_length, position_dim). + :return: collsion costs on the trajectories. + """ + assert self.cost_map is not None + if x.device != self.device or x.dtype != self.dtype: + x = x.to(self.device, self.dtype) + + # project to cell map + x_occ = (x - self.origin_tensor) / self.cell_size + x_occ = torch.round(x_occ).long().to(self.device) + + x_occ[..., 0] = torch.clamp(x_occ[..., 0], 0, self.cost_map.shape[0] - 1) + x_occ[..., 1] = torch.clamp(x_occ[..., 1], 0, self.cost_map.shape[1] - 1) + + # collision check + collisions = self.cost_map[x_occ[..., 1], x_occ[..., 0]] + + return collisions \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py new file mode 100644 index 00000000..30995175 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py @@ -0,0 +1,244 @@ +import torch +import numpy as np +from typing import Tuple +from mppi_controller.MPPI import MPPI +from mppi_controller.cost_map_tensor import CostMapTensor + +import time + +@torch.jit.script +def angle_normalize(x) -> torch.Tensor: + return ((x + torch.pi) % (2 * torch.pi)) - torch.pi + +class mppi_controller: + def __init__(self, config, debug=False, device=torch.device("cuda"), dtype=torch.float32): + self.config = config + self.debug = debug + # device and dtype + if torch.cuda.is_available() and device == torch.device("cuda"): + self._device = torch.device("cuda") + else: + self._device = torch.device("cpu") + self._dtype = dtype + + self.u_min = torch.tensor(self.config["u_min"], device=self._device, dtype=self._dtype) + self.u_max = torch.tensor(self.config["u_max"], device=self._device, dtype=self._dtype) + self.sigmas = torch.tensor(self.config["sigmas"], device=self._device, dtype=self._dtype) + + # solver + self.solver = MPPI( + horizon=self.config["horizon"], + num_samples=self.config["num_samples"], + dim_state=4, + dim_control=2, + dynamics=self.dynamics, + cost_func=self.cost_function, + u_min=self.u_min, + u_max=self.u_max, + sigmas=self.sigmas, + lambda_=self.config["lambda"], + auto_lambda=self.config["auto_lambda"], + ) + + # model parameter + self.delta_t = torch.tensor(self.config["delta_t"], device=self._device, dtype=self._dtype) + self.vehicle_L = torch.tensor(self.config["vehicle_L"], device=self._device, dtype=self._dtype) + self.V_MAX = torch.tensor(self.config["V_MAX"], device=self._device, dtype=self._dtype) + + # cost weights + self.Qc = self.config["Qc"] # contouring error cost + self.Ql = self.config["Ql"] # lag error cost + self.Qv = self.config["Qv"] # velocity cost + self.Qo = self.config["Qo"] # obstacle cost + self.Qin = self.config["Qin"] # input cost + self.Qdin = self.config["Qdin"] # input rate cost + + self.current_path_index = 0 + + # reference indformation (tensor) + self.reference_path: torch.Tensor = None + self.cost_map: CostMapTensor = None + + def update(self, state: torch.Tensor, racing_center_path: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Update the controller with the current state and reference path. + Args: + state (torch.Tensor): current state of the vehicle, shape (4,) [x, y, yaw, v] + racing_center_path (torch.Tensor): racing center path, shape (N, 3) [x, y, yaw] + Returns: + Tuple[torch.Tensor, torch.Tensor]: action sequence tensor, shape (horizon, 2) [accel, steer], state sequence tensor, shape (horizon + 1, 4) [x, y, yaw, v] + """ + + # reference + self.reference_path, self.current_path_index = self.calc_ref_trajectory( + state, racing_center_path, self.current_path_index, self.solver._horizon, DL=self.config["DL"], lookahead_distance=self.config["lookahead_distance"], reference_path_interval=self.config["reference_path_interval"] + ) + + if self.reference_path is None: + raise ValueError("reference path, obstacle map, and lane map must be set before calling solve method.") + + # solve + start = time.time() + action_seq, state_seq = self.solver.forward(state=state) + end = time.time() + solve_time = end - start + + if self.debug: + print("solve time: {}".format(round(solve_time * 1000, 2)), " [ms]") + + return action_seq, state_seq + + def get_top_samples(self, num_samples = 300) -> Tuple[torch.Tensor, torch.Tensor]: + return self.solver.get_top_samples(num_samples=num_samples) + + def set_cost_map(self, cost_map: CostMapTensor) -> None: + self.cost_map = cost_map + + def dynamics( + self, state: torch.Tensor, action: torch.Tensor + ) -> torch.Tensor: + """ + Update robot state based on differential drive dynamics. + Args: + state (torch.Tensor): state batch tensor, shape (batch_size, 3) [x, y, theta, v] + action (torch.Tensor): control batch tensor, shape (batch_size, 2) [accel, steer] + delta_t (float): time step interval [s] + Returns: + torch.Tensor: shape (batch_size, 3) [x, y, theta] + """ + + # Perform calculations as before + x = state[:, 0].view(-1, 1) + y = state[:, 1].view(-1, 1) + theta = state[:, 2].view(-1, 1) + v = state[:, 3].view(-1, 1) + accel = torch.clamp(action[:, 0].view(-1, 1), self.u_min[0], self.u_max[0]) + steer = torch.clamp(action[:, 1].view(-1, 1), self.u_min[1], self.u_max[1]) + theta = angle_normalize(theta) + + dx = v * torch.cos(theta) + dy = v * torch.sin(theta) + dv = accel + dtheta = v * torch.tan(steer) / self.vehicle_L + + new_x = x + dx * self.delta_t + new_y = y + dy * self.delta_t + new_theta = angle_normalize(theta + dtheta * self.delta_t) + new_v = v + dv * self.delta_t + + # Clamp velocity + new_v = torch.clamp(new_v, -self.V_MAX, self.V_MAX) + + result = torch.cat([new_x, new_y, new_theta, new_v], dim=1) + + return result + + + def cost_function(self, state: torch.Tensor, action: torch.Tensor, info: dict) -> torch.Tensor: + """ + Calculate cost function + Args: + state (torch.Tensor): state batch tensor, shape (batch_size, 4) [x, y, theta, v] + action (torch.Tensor): control batch tensor, shape (batch_size, 2) [accel, steer] + Returns: + torch.Tensor: shape (batch_size,) + """ + # info + prev_action = info["prev_action"] + t = info["t"] # horizon number + + # path cost + # contouring and lag error of path + ec = torch.sin(self.reference_path[t, 2]) * (state[:, 0] - self.reference_path[t, 0]) \ + -torch.cos(self.reference_path[t, 2]) * (state[:, 1] - self.reference_path[t, 1]) + el = -torch.cos(self.reference_path[t, 2]) * (state[:, 0] - self.reference_path[t, 0]) \ + -torch.sin(self.reference_path[t, 2]) * (state[:, 1] - self.reference_path[t, 1]) + + path_cost = self.Qc * ec.pow(2) + self.Ql * el.pow(2) + + # velocity cost + v = state[:, 3] + v_target = self.reference_path[t, 3] + velocity_cost = self.Qv * (v - v_target).pow(2) + + # compute obstacle cost from cost map + pos_batch = state[:, :2].unsqueeze(1) # (batch_size, 1, 2) + obstacle_cost = self.cost_map.compute_cost(pos_batch).squeeze(1) # (batch_size,) + obstacle_cost = self.Qo * obstacle_cost + + # input cost + input_cost = self.Qin * action.pow(2).sum(dim=1) + input_cost += self.Qdin * (action - prev_action).pow(2).sum(dim=1) + + cost = path_cost + velocity_cost + obstacle_cost + input_cost + + return cost + + def calc_ref_trajectory(self, state: torch.Tensor, path: torch.Tensor, + cind: int, horizon: int, DL=0.1, lookahead_distance=1.0, reference_path_interval=0.5 + ) -> Tuple[torch.Tensor, int]: + """ + Calculate the reference trajectory for the vehicle. + + Args: + state (torch.Tensor): current state of the vehicle, shape (4,) [x, y, yaw, v] + path (torch.Tensor): reference path, shape (N, 4) [x, y, yaw, target_v] + cind (int): current index of the vehicle on the path + horizon (int): prediction horizon + DL (float): resolution of the path + lookahead_distance (float): distance to look ahead + reference_path_interval (float): interval of the reference path + + Returns: + Tuple[torch.Tensor, int]: reference trajectory tensor, shape (horizon + 1, 4) [x, y, yaw, target_v], index of the vehicle on the path + """ + + def resample_path(path, DL): + # Calculate the number of segments needed for each pair of points + distances = torch.norm(path[1:, :2] - path[:-1, :2], dim=1) + num_points = torch.ceil(distances / DL).to(torch.int64) + + # Create a tensor to store the new resampled path points + total_points = num_points.sum() + 1 # Include the first point + new_path = torch.zeros((total_points, path.shape[1]), dtype=path.dtype, device=path.device) + + # Initialize the first point + new_path[0] = path[0] + + # Generate all new points at once + start_idx = 1 + for i in range(len(path) - 1): + segment_length = num_points[i].item() + if segment_length == 0: + continue + + t = torch.linspace(0, 1, segment_length + 1, device=path.device)[1:] # Skip the first point to avoid duplication + interpolated_points = (1 - t).unsqueeze(1) * path[i] + t.unsqueeze(1) * path[i + 1] + new_path[start_idx:start_idx + segment_length] = interpolated_points + start_idx += segment_length + + return new_path + + # Resample the path with the specified DL + path = resample_path(path, DL) + ncourse = len(path) + xref = torch.zeros((horizon + 1, state.shape[0]), dtype=state.dtype, device=state.device) + + # Calculate the nearest index to the vehicle + ind = torch.argmin(torch.norm(path[:, :2] - state[:2], dim=1)).item() + # Ensure the index is not less than the current index + ind = max(cind, ind) + + # Generate the rest of the reference trajectory + travel = lookahead_distance + + for i in range(horizon + 1): + travel += reference_path_interval + dind = int(round(travel / DL)) + + if (ind + dind) < ncourse: + xref[i] = path[ind + dind] + else: + xref[i] = path[-1] + + return xref, ind \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py new file mode 100644 index 00000000..a0153179 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py @@ -0,0 +1,250 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, Point, Quaternion +from autoware_auto_planning_msgs.msg import Trajectory, TrajectoryPoint +from autoware_auto_control_msgs.msg import AckermannControlCommand +from nav_msgs.msg import OccupancyGrid +from rclpy.qos import QoSProfile +import math +import torch +import numpy as np +import tf_transformations +from mppi_controller.mppi_controller import mppi_controller +from mppi_controller.cost_map_tensor import CostMapTensor + +class MppiControllerNode(Node): + + def __init__(self): + super().__init__('mppi_controller_node') + + # parameters + # declare + self.declare_parameter('horizon', 25) + self.declare_parameter('num_samples', 4000) + self.declare_parameter('u_min', [-2.0, -0.25]) + self.declare_parameter('u_max', [2.0, 0.25]) + self.declare_parameter('sigmas', [0.5, 0.1]) + self.declare_parameter('lambda', 1.0) + self.declare_parameter('auto_lambda', False) + self.declare_parameter('DL', 0.1) + self.declare_parameter('lookahead_distance', 3.0) + self.declare_parameter('reference_path_interval', 0.85) + self.declare_parameter('Qc', 2.0) + self.declare_parameter('Ql', 3.0) + self.declare_parameter('Qv', 2.0) + self.declare_parameter('Qo', 10000.0) + self.declare_parameter('Qin', 0.01) + self.declare_parameter('Qdin', 0.5) + self.declare_parameter('delta_t', 0.1) + self.declare_parameter('vehicle_L', 1.0) + self.declare_parameter('V_MAX', 8.0) + # get + config = { + "horizon": self.get_parameter('horizon').get_parameter_value().integer_value, + "num_samples": self.get_parameter('num_samples').get_parameter_value().integer_value, + "u_min": self.get_parameter('u_min').get_parameter_value().double_array_value, + "u_max": self.get_parameter('u_max').get_parameter_value().double_array_value, + "sigmas": self.get_parameter('sigmas').get_parameter_value().double_array_value, + "lambda": self.get_parameter('lambda').get_parameter_value().double_value, + "auto_lambda": self.get_parameter('auto_lambda').get_parameter_value().bool_value, + "DL": self.get_parameter('DL').get_parameter_value().double_value, + "lookahead_distance": self.get_parameter('lookahead_distance').get_parameter_value().double_value, + "reference_path_interval": self.get_parameter('reference_path_interval').get_parameter_value().double_value, + "Qc": self.get_parameter('Qc').get_parameter_value().double_value, + "Ql": self.get_parameter('Ql').get_parameter_value().double_value, + "Qv": self.get_parameter('Qv').get_parameter_value().double_value, + "Qo": self.get_parameter('Qo').get_parameter_value().double_value, + "Qin": self.get_parameter('Qin').get_parameter_value().double_value, + "Qdin": self.get_parameter('Qdin').get_parameter_value().double_value, + "delta_t": self.get_parameter('delta_t').get_parameter_value().double_value, + "vehicle_L": self.get_parameter('vehicle_L').get_parameter_value().double_value, + "V_MAX": self.get_parameter('V_MAX').get_parameter_value().double_value, + } + + self.get_logger().info(f'config: {config}') + + # publisher + # control command + self.pub_cmd = self.create_publisher(AckermannControlCommand, 'output/control_cmd', 1) + # planned path + self.pub_planned_path = self.create_publisher(Trajectory, 'output/planned_path', 1) + # debug path + self.pub_debug_path = self.create_publisher(Trajectory, 'debug/path', 1) + + # subscriber + # state + self.sub_kinematics = self.create_subscription( + Odometry, + 'input/kinematics', + self.kinematics_callback, + 1 + ) + # reference trajectory + self.sub_trajectory = self.create_subscription( + Trajectory, + 'input/reference_trajectory', + self.trajectory_callback, + 1 + ) + # costmap + self.sub_costmap = self.create_subscription( + OccupancyGrid, + 'input/costmap', + self.costmap_callback, + 1 + ) + + # device and dtype + self.device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") + self.dtype = torch.float32 + + # mppi controller + self.controller = mppi_controller(config=config, debug=True, device=self.device, dtype=self.dtype) + + self.odometry: Odometry = None + self.trajectory: Trajectory = None + self.costmap: OccupancyGrid = None + + self.timer = self.create_timer(0.03, self.on_timer) + + def kinematics_callback(self, msg : Odometry): + self.odometry = msg + + def trajectory_callback(self, msg : Trajectory): + self.trajectory = msg + + def costmap_callback(self, msg : OccupancyGrid): + self.costmap = msg + + def zero_ackermann_control_command(self): + cmd = AckermannControlCommand() + now = self.get_clock().now().to_msg() + cmd.stamp = now + cmd.longitudinal.stamp = now + cmd.longitudinal.speed = 0.0 + cmd.longitudinal.acceleration = 0.0 + cmd.lateral.stamp = now + cmd.lateral.steering_tire_angle = 0.0 + return cmd + + def on_timer(self): + if not self.subscribe_message_available(): + return + + cmd = self.zero_ackermann_control_command() + + # convert tensor + # state + state_tensor = torch.tensor([ + self.odometry.pose.pose.position.x, + self.odometry.pose.pose.position.y, + tf_transformations.euler_from_quaternion([ + self.odometry.pose.pose.orientation.x, + self.odometry.pose.pose.orientation.y, + self.odometry.pose.pose.orientation.z, + self.odometry.pose.pose.orientation.w + ])[2], + self.odometry.twist.twist.linear.x + ], dtype=self.dtype, device=self.device) + # reference path + reference_path_tensor = torch.tensor([ + [point.pose.position.x, + point.pose.position.y, + tf_transformations.euler_from_quaternion([ + point.pose.orientation.x, + point.pose.orientation.y, + point.pose.orientation.z, + point.pose.orientation.w + ])[2], + point.longitudinal_velocity_mps] for point in self.trajectory.points + ], dtype=self.dtype, device=self.device) + # convert OccupancyGrid to tensor + costmap_tensor: CostMapTensor = CostMapTensor( + cost_map=torch.tensor(self.costmap.data, dtype=self.dtype, device=self.device).reshape(self.costmap.info.height, self.costmap.info.width), + cell_size=self.costmap.info.resolution, + origin=(self.costmap.info.origin.position.x, + self.costmap.info.origin.position.y), + device=self.device, + dtype=self.dtype + ) + # set cost map + self.controller.set_cost_map(costmap_tensor) + + # update controller + action_seq, state_seq = self.controller.update(state_tensor, reference_path_tensor) + # get top samples + top_samples = self.controller.get_top_samples(num_samples=300) + + # convert numpy + state = state_seq.cpu().numpy()[0] + action = action_seq.cpu().numpy()[0] + + # publish control command + cmd.longitudinal.speed = float(state[0, 3]) + cmd.longitudinal.acceleration = float(action[0]) + cmd.lateral.steering_tire_angle = float(action[1]) + self.pub_cmd.publish(cmd) + + # publish planned path + planned_path = Trajectory() + planned_path.header.stamp = self.get_clock().now().to_msg() + planned_path.header.frame_id = 'map' + planned_path.points = [ + TrajectoryPoint( + pose=Pose( + position=Point(x=float(point[0]), y=float(point[1])), + orientation=Quaternion( + x=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[0], + y=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[1], + z=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[2], + w=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[3] + ) + ), + longitudinal_velocity_mps=float(point[3]) + ) + for point in state] + + self.pub_planned_path.publish(planned_path) + + # publish debug path + debug_path = Trajectory() + debug_path.header.stamp = self.get_clock().now().to_msg() + debug_path.header.frame_id = 'map' + debug_path.points = [ + TrajectoryPoint( + pose=Pose( + position=Point(x=float(point[0]), y=float(point[1])), + orientation=Quaternion( + x=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[0], + y=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[1], + z=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[2], + w=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[3] + ) + ), + longitudinal_velocity_mps=float(point[3]) + ) + for point in self.controller.reference_path.cpu().numpy()] + self.pub_debug_path.publish(debug_path) + + + def subscribe_message_available(self): + if not self.odometry: + self.get_logger().info('odometry is not available', throttle_duration_sec=1) + return False + if not self.trajectory: + self.get_logger().info('trajectory is not available', throttle_duration_sec=1) + return False + if not self.costmap: + self.get_logger().info('costmap is not available', throttle_duration_sec=1) + return False + return True + +def main(args=None): + rclpy.init(args=args) + node = MppiControllerNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml new file mode 100644 index 00000000..e35b459d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml @@ -0,0 +1,16 @@ + + + + mppi_controller + 0.0.0 + TODO: Package description + eguchi + TODO: License declaration + + rclpy + rclpy + + + ament_python + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller new file mode 100644 index 00000000..e69de29b diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg new file mode 100644 index 00000000..7d2409e1 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mppi_controller +[install] +install_scripts=$base/lib/mppi_controller \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py new file mode 100644 index 00000000..37ccad01 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py @@ -0,0 +1,33 @@ +import subprocess +import sys + +# dependency +subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'setuptools==58.0.4', '--quiet']) +subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'torch', '--index-url', 'https://download.pytorch.org/whl/cpu', '--quiet']) +subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'transforms3d', '--quiet']) + +from setuptools import setup, find_packages +package_name = 'mppi_controller' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', ['config/mppi_controller.param.yaml']), + ], + install_requires=['setuptools', 'scipy'], + zip_safe=True, + maintainer='michikuni eguchi', + maintainer_email='egrt117@gmail.com', + description='MPPI controller', + license='License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mppi_controller_node = mppi_controller.mppi_controller_node:main', + ], + }, +) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index 2d248d76..9e75c143 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -5,9 +5,13 @@ Panels: Property Tree Widget: Expanded: - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Planning1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 + - /Planning1/Costmap1/Map1 + - /Planning1/MPPI1 + - /Planning1/MPPI1/Trajectory1 Splitter Ratio: 0.557669460773468 - Tree Height: 242 + Tree Height: 158 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -865,7 +869,7 @@ Visualization Manager: - Class: rviz_common/Group Displays: - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 + Color Border Vel Max: 8 Enabled: true Name: ScenarioTrajectory Topic: @@ -889,7 +893,7 @@ Visualization Manager: Color: 0; 0; 0 Constant Color: false Value: true - Width: 2 + Width: 0.5 View Point: Alpha: 1 Color: 0; 60; 255 @@ -1941,7 +1945,7 @@ Visualization Manager: Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map - Color Scheme: map + Color Scheme: costmap Draw Behind: false Enabled: true Name: Map @@ -1962,6 +1966,51 @@ Visualization Manager: Value: true Enabled: true Name: Costmap + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 8 + Enabled: true + Name: Trajectory + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 11 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/output/mppi_planned_path + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 0.5 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + Enabled: true + Name: MPPI Enabled: true Name: Planning - Class: rviz_common/Group @@ -2170,11 +2219,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 10.328941345214844 + Scale: 7.122405529022217 Target Frame: viewer Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 + X: 2.5461606979370117 + Y: -8.487200736999512 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 @@ -2220,12 +2269,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1376 + Height: 1043 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -2234,6 +2283,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3370 - X: 70 - Y: 27 + Width: 1920 + X: 0 + Y: 32 diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml index eda86a3a..9969163f 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml @@ -9,7 +9,7 @@ - + From a16db558f8fcd6bf9d5e97bed8d7155dcc76de93 Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:08:24 +0900 Subject: [PATCH 04/12] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../mppi_controller/config/mppi_controller.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml index 96d2990a..15fa92cd 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -10,7 +10,7 @@ auto_lambda : false # reference path DL : 0.1 - loolahead_distance : 3.0 + lookahead_distance : 3.0 reference_path_interval : 0.85 # cost weights Qc : 2.0 From 257cd04032778c7ea4283f9fe89932dc6187ca9f Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:08:48 +0900 Subject: [PATCH 05/12] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../mppi_controller/mppi_controller/mppi_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py index 30995175..7a65192f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py @@ -55,7 +55,7 @@ def __init__(self, config, debug=False, device=torch.device("cuda"), dtype=torch self.current_path_index = 0 - # reference indformation (tensor) + # reference information (tensor) self.reference_path: torch.Tensor = None self.cost_map: CostMapTensor = None From 089dc4ddf81476b3a7a7002769643962828dc74e Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:09:06 +0900 Subject: [PATCH 06/12] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../mppi_controller/mppi_controller/cost_map_tensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py index fd977e60..747bc749 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py @@ -30,7 +30,7 @@ def compute_cost(self, x: torch.Tensor) -> torch.Tensor: """ Check collision in a batch of trajectories. :param x: Tensor of shape (batch_size, traj_length, position_dim). - :return: collsion costs on the trajectories. + :return: collision costs on the trajectories. """ assert self.cost_map is not None if x.device != self.device or x.dtype != self.dtype: From 0259b920429315892c186da8d2bb5d391e782e77 Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:09:15 +0900 Subject: [PATCH 07/12] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../aichallenge_submit/mppi_controller/mppi_controller/MPPI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py index 18ca6044..3e2dfd52 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -335,7 +335,7 @@ def forward( # use only N step optimal action seq optimal_action_seq = filtered_action_seq[-self._horizon :] - # predivtive state seq + # predictive state seq expanded_optimal_action_seq = optimal_action_seq.repeat(1, 1, 1) optimal_state_seq = self._states_prediction(state, expanded_optimal_action_seq) From 925af61a45a0770de214e9697a41427b39734c6a Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:09:24 +0900 Subject: [PATCH 08/12] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../aichallenge_submit/mppi_controller/mppi_controller/MPPI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py index 3e2dfd52..d0fa4af7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -377,7 +377,7 @@ def get_samples_from_posterior( assert num_samples <= self._num_samples # posterior distribution of MPPI - # covaraince is the same as noise distribution + # covariance is the same as noise distribution posterior_distribution = MultivariateNormal( loc=optimal_solution, covariance_matrix=self._covariance ) From aa88f2ab39b5d8d4283fc2c44d788e072898b9ea Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 20:21:41 +0900 Subject: [PATCH 09/12] add some spell to cspell.json --- .cspell.json | 13 ++++++++++++- .github/CODEOWNERS | 1 + .../mppi_controller/.gitignore | 1 + .../__pycache__/MPPI.cpython-310.pyc | Bin 10093 -> 0 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 165 -> 0 bytes .../__pycache__/cost_map_tensor.cpython-310.pyc | Bin 1831 -> 0 bytes .../__pycache__/mppi_controller.cpython-310.pyc | Bin 7808 -> 0 bytes .../mppi_controller_node.cpython-310.pyc | Bin 6746 -> 0 bytes 8 files changed, 14 insertions(+), 1 deletion(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller_node.cpython-310.pyc diff --git a/.cspell.json b/.cspell.json index 3c1715b8..7fef0397 100644 --- a/.cspell.json +++ b/.cspell.json @@ -104,6 +104,17 @@ "wpedantic", "xacro", "xyzrpy", - "zcvf" + "zcvf", + "mppi", + "Qdin", + "Kohei", + "MPPI", + "Savitzky", + "Golay", + "reparameting", + "tempature", + "rsample", + "coeffs", + "softplus" ] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 379992ee..13913d57 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -12,3 +12,4 @@ aichallenge/workspace/src/aichallenge_submit/racing_kart_description/** @Autumn6 aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/** @booars/aic2024-developers aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore new file mode 100644 index 00000000..7e99e367 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc deleted file mode 100644 index 624e444edbe033ca6540a9d6462d8a7a15a6e5de..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10093 zcmbtaON<*=cCA0Z&Hjk$pZqDwmMz*+%aZ?XjctwXamE8_6ic2-+Hq=1d_{J#i&fOC zVoT~NW`an@APEF`6(AV|VIx2gXOZt}Hpw~&5Cqu_@Q_U~D=o6iN>1E4_Z5p|cT0+q zu))js|J--qz31N7(acO)!Dr^)zqHnJndy&#%)5%B#v4d5WkX>q(*m`tZD=Z` z^+4|$8%EdMFuT@ zRi#nfsh7VOZF;`_D^bYY)Asq7&cD1?zE5jusG{PAVU)OuABAy47sdD7AaJh-o-nWV zd-yTl_nXOOG+emU4-$Xd<-VJEmm}VFgC>nJ{V8S{HNK04r);R$m&Vi$ooS4|1~WZl zXfkV8^^6Wy+bXiReyX%oX0zjWXRx;u?3ugDM$xCT3+xnY+%-2!>|5*w_TpV*qs-oB zYwRpi74{B0$Ic^FW$&_A*=tD6u#4#gwgCX_x=l%ENn*H--$w@`(Yv~VZZCdZWnBgMTz;yfbpKF zP_miLEsE@R=yv^P{NR=RC_eo(iFk8!?V1qx4UjE3W3|b&Rj>Btsru-KewMmPF5(4>roVlig&9Q zL|lrOnCZu!6Stj~A0!?ZH6(BPA&YK0vA^Sq8RUCWaNCJEL$wim9upS#l0FZGaXAXT z=8>G;_+7-egfDom#x>AHxuXo#T}4$AZK$UUyQ-?Bg^rOLEp+A(^I8W{F(B{+8vs49;M}! z>ME%QT1`>!%%lcrEj2-BQwwx1Eez*(mEi)uM`#3m%e&O0o)#Zbb2KeIqGmBIqvuju z1zk>OKv&Wl=xRC(dJOw|0^?WkMXCJC`IvE!R`eux^i*16<`HFA-M^a7lh7*G=h`%F z=@-D`&eIs_NqqB>XrFd+oE1=3xude-7zqJI{kY$4d98{Oe) zFU|SVWb8xzNG{H$I2q@BTKPidpQG)|!&k6=NL}YO%&^oZzKzG*&A9P8YTxL*iJI5j zDl4;!p2uEGvL^Htp!VPd43BGtFvbGh5KM+#|uS_zKI?7$1#$!4b6rn>aT>e7K0 z^Z2$K?CYKv+NWX>iSl6N2y3m<0ZL&Qn+lRIu>D~kFN2P~*j1)AQk5YD*?8>2rV@SFtFY>EI09@6>-~ho16JW zHtbKvrb8!;bJGj6Q}e_2p}zZ! z!@B(m)_&F9_LH5h+h;zE0{6C^*^NUT_nVP*SZ%T?nNZPIA3lm5xV#NP@?$&hMp3ef z?bw(4IHv2K9WnN>X(rYQ7XX$jjJiHxgS1zYtB;uKDzH*;V6KA~la8n9L8jy{Y`o3; zp^atPaTIJngx4&&-J|wy;@Xt_23^RauG?2ZsR=7v znK0#q_cb)Sd|w4cogPF@G2e_NDh!(*;6cLuTVJZ8n%zz(-gJ9j`^WXa{P5p*?!E&U zsRg|8mN4T!#7Pt#s1!eO03qaJM5XJ7eK&BV?KfrTY*|%5cHC{(m)Mj)M=LEj`O12n z;Faab6=KjFbyb}y)gj8x1aehndUVl5$;m+%8jWwQFq;8D2QeVQZ4p`$VGem$vtK}= z0-#HVg>u|eb*3(Xs5$$@TFesunFc*IDwxQXFn2s2#iD{`%8v zhuhOlOzwtlPZUt>@z~=QG3baBk_y09W1PzMK3#rcT$NaBghR9Q5e1$d7uM@a6_xdj zi3Px`V$s2A;qG7eSl%G^Zp5PQK&5Z{Q6Gns^+PdQ$D@nuwCUnvpldVYH|iN%5w^%5U9Y0^eiooUFag`p^2Au~tH ziG;H)tVDulF@ualN=&LSValxA z_s_b1bCYxj4)^TMh;PL`x9Od|-uDA`w%hCZ;HY$2pj{5-$?sX~-R9>oEiF49f|Oq@ z>vQ^?I;SohHMOeEsg`Q#_>TToRrC>=sZz=|E9EFgKMV>0y~Au21cW12*B2kQ$=2Sze>ULC8ykG&Xf z9x2>hrtM6oZJbQsgLa#k1bz~;^9IP}Mv=Y|PkG-Pn*kC#C~f3rAhOrw`PyjvfT=%`H7Xi8Mh{YfI;6 zQgv9$+wN)u>Q^3C;Mi1ml>xDSM#AY?rb*a7m%vHtEJ(-c$g)yb?x+KU+@#JNgS+;* zwxTR40}~LqP7YINE;X1eZD&pwf%`?26TDx-cUc1aqgcfXau}BYeP;(jtbZ0)BsM{kldG1yVT0ttq@m^X1VFo=}}RxFDZEsI5fWnjy;8b3p4}1Ip2&y z;gL1OzeB5BQ3j=Cvd$3ew3xYN@PDH@mz7Xu<^axK-ZL%dt3Xe3@bZ;UmsjCnz=gY@ zz(Lel0ly{u7V&H0w}fA1NbcM>=0&Q3=Y^a{Ee#{+sU6q9=33zo#QAgPWQO+ zuag2epN)lPAaoX81J;do82msKXc;rwE4gBtoigB9wm3dg7hd7^W8HZ4?W zWhaj*kPE7TTPa(@sT`7P3F(3SkSrdpkJ=1o(^)CSr{JFbB00u%DM4$4IkajFU&ulZ zofkW&hi6c-wyO^2G3Hr0*L*(HV1dCMMtFc+;+H5qu>{E<)YJO#9E~8yI!~!D)Ej?- zHZNym4PV(sopTboSHI9WdP`^awRE;kvW-2Ev)g%HI=eZC^ueN(p%ZeYiv!$};Tvf# z8B25?81GF;^y275P}sQ1ixsCUEe&e7w)4xe+PzBXR;t~Rc^`WvJsu$r*_~+J zi@Y7Lk2{iwDpjI=YC1^OfV0)-+zXSecGCT08A&=Y!vCs5*KOH^9LOp6b!2vJ_*SDu zJjzI%fyT`&1tl+MM5F=y7Nw*;e4A32L4bSigB98m{kU7e`^^SZ2X* z&?uZ5&C-O^*te8y!d!1g0Tc6n2&WR$vOMT34;7di!GKs)M~GSG_%@A}$p+k4?-oyJ zjy{!EBt{%bNe##rfmw)2Hp)T{ibtbC429~5(gf<(q(d{|QNioQJ{%ii1W^nKk86fp zdmpkFM>1b~f1&D&V_+p@9_0_2cRnwZj3YoRYI#w%yo1=M04PHT^*7jO{Ldf@<+8S@ zE(4a@8N2`$YkAms_OrV(IVRG<3VuqKdmS+A*ebs2v}tq8bDfRkEj>fC=@ zRuKSmLDRmT+gnCD2egK2{g+{_s>WWmprM7fw_GrE>SKLvm_^K2#>{oJU%+@ZqKk%R z0QhQOYliieQ7U6aHNf8mwZ1oJ)^w~;FCxE;@}jX*mH2J6$tJnZ`G)`;eI*$CO(Z1P zQnu8%Ac2@V%uubm3m`V2w3(7RCc~u56aZ7Ab}T?6yc^xi>Wl*=1gZ?Fa`l~Gj5X(n zgmWW=-Lj{MiH1C^V5j_N`xKmPXxFI#90AgN6be7Iy<{@j|6AQoEm7)OKy#nlBX@8z z&o$`AGV|i57c#_^8ih=ANu_9DjIB(;z%|A#_F}25L;=Acyrm=!GrQ5`>WKS!OizK^dOKt{v(*YV1PI$24B8IH#Ajvbvx_ZnT}cl!XJ0#awh)(!>0E{pM>Pt=7a0~d_ z$^_&xe*t50vR63|$=TxH1HqGzl$FfY`Iw4-KWisFnlGt9Dn-f1#U2zKG#`BWeW8a! z&d&Kp0Qbz5ie~J4C7=F*hO;l5z;pG-C2HrX8SGpMFAY}mY zB-pPEi+L;D)KM#ps%7cvGJvtscqqk;d22Wq&!o~@y(}SwL=(~jZIq^bPl*Sl{gt3# zV*jOKCYziKyF97CMu8}gHFYd)(}81)>n%K(!;0JC*aEOEJ@xU zHkhI^9qTGj;#~XqPp`%#YEqzkNTe!L7Zs-A)s9}$v@Mm_5`|zlUhwexy{s^G6}k}G z4m<;(2jr9|MA{K=%=e4|X+jfkZg_JBC-Kq@2i7F*I4nT$Rq0fb&Mc$_0Mep+8oL}T z$oHaduMgEm!GMQ`bb(P3(ej%s54cLLefyiEg#Bwyp}`S_)NBkI2$K3_xrcy z#d!A@rehTL##c}7AsU6u<3ztf}%>fV*`c5|8aBB5yQivN`78oOhdaa=e z^|C1YVG~`5qm7~{|G*9U-UpmVJVUt_qIUjMf@OUiqI=iF#4gda>7M5d~8zYAUQOPfbVJ)YHj@8`hCS8W{O{3mpU9KAlOC5MPfJap67E3kGqHy8hptX_^1z1Vc|q(+ z-M}l;CS`4uOF@U0$3IoTe`Y2;S8bBc1wS=W2CISn5Yjf- zI}&x0RKUh zMDOhX9*Vae8VUdGP@XL9pbY*VI{utwkYa?@+8P402-EJBjh)p@_8#WQG)`oJZ0XOD zY0f-58Pb+zGOjIC^HjS)ghIufBs~jrFH%azx@BsavNE+oDY6S$==%vu$@sS{lRXg&dz$Mz+%FdU<$qCc4iD`_Iv97XEd|!)sRAaquME4=7!AoEv!3 z8r2jWheb`t$)tr8pD4)xo_YDa4AgV7e|$*f&x!mM5yD!Wv=ApT5Hq7Uf~FS)7%8L* zNdihmL`b*LPGZ7zMa59Hc{nZdk4!k6qW@5SQmkv$>YnnG^11TS@{#hw!c($eqsIRU zy=W7yK0Wwi4vh*dJTpQsa~&B@2sAsQ^Hf*NVlZLIe?-~mv+Okdi`TkS|3HQZCi)Mu q?4ioS|7C$$a`vh1$UV_^MDaq#&bM&`GH`7uRih3^uZq7#{eJ+4y2-}? diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index bd4bef9f354ab13a6bfeb727b79715c2440c1371..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 165 zcmd1j<>g`k0^Z&WX(0MBh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6v5KQS{oBQYl@ zH7`9?zdXMvySN}RIaNQYG&3hfKewPDGd?*#ucRm+sG2 SKczG$)edAsF%ytrVE_QkFDWem diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc deleted file mode 100644 index 3f99c96c85732caae4e643e101ce192d77ce0711..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1831 zcmZux&1)n@6tAlO%p}>(W+lX3g<=mf4IzOQFGG-p90b95aPcrAO>I|Arf2%g>K@I8 z&P8+e>@{=rUl9B!Jl55N?4RJG;(OIIlMY%?ufBfo)vNb@ugcxs0fF|@N54(>144es zMZY<)cmcis0E8rxW@L;b%V^HVjFL}?bfkMjr27LKJCc4uBJT}g$$3PL_i|BXD#2yH zZNcIN^!iH>ij1iwV8e0_DpY|AChT1zeHlzb8Op62Z@eW3 z7UQFwxN=+W+_>X`48Vz9vwK$7&1a%|sfxPP5&Hu!vrW4&7&lo+VZsDJaV{#em8dL> z>-0()ztrhPS{PreW}%Do#9wLRfO8M}8T9%&h?ce_CM^R=Tj$zo*vx62)@?mWe{^8i zg*l16)&uEF2F^KRP6-+CRYM;*_`nMFZiuf46L2 zSEwNx!EuUi<2_v<@30XwerMKrR_8nlG-gr*>51`d(2U=yncdFmxLazh5WNi|@{L=o zY;vcmLp<96ekOVkHr{@IBGP0kvP=~h>g2mp&+AGg>g3fT&E!d5RcV}*MWZ3dO8;lQ z&gJU<@WzB@5}OIn=J@;{V=%9TJXk1gynE|bIzHs48Y8QO_Qh}cO= zEG`9%tef{?r``wgz(1sWbk8}U&)741;JEDS$$gUb)uL@=Xd!YH$7T@6c_|ke&UfPY zn}x`_6YIDw78BV2;lt;~@1k!!yP=V-8t>Qdp}=D53G`PxAdtv4B0|Ev5bD5v>oWEa zbJ*Vlm;J?p!JkhKEfRuZ!{x*80DowA0h;KUwYS(=d7hrv&dxpnf)EIhkTkMvg&U(liDK-8#Yk33e#ns^%aT1p#wB9DVmWsmifWu1cz8-g3#s2Q%N_v+t!%rYhwH zx~JdW)7}5~fB!#eqv2}!{rH9dXn*n-n)aWRnf#f^d<7wEBjOroiMCxn*|v_no){f- z+hkN`Bv!}XwpH3pDjjFrQE8S`x2uu1@9x*Qg=f93bDLN0o2P8M@pX+myn0{b)l+@D z$=L^5%YBHsw5=AC)`$IG5+Px}`>k)ijc4P{G|S%&dLKsJEEUogQQjBbkWq)hkJ1Lh zn+VxuM1_`X`|J|dPS0g+MdepD%%pEU)X}1)ODj)BxF_{qj9J*>r<$OB`Tcv>f;ij@ zk|gTxM%V78;^V9rgweH~ew^@Yon9~Y!?c@=6fMM)@|(RQxe#=BlgRI;q7x+Xaa5&M z(i$%7qK-$1F!gI7_Z5WfzYs;*HscyLo<+u)$@OzQ_pP?U4Q}3N+cvh>*spL~#g)jR zCxy1YrnPOg(dKrQKhJ0DbYA22`_{J08&Qo{c(ZNt1-^LS+^!GCyf7G(Mzi@6zlgfV zRNZpa#JE)N@Mri6>KCeO+#9sHtomPx7Wq2DaQ;QLGM7KgpTq1IcoT>6f?Sx{*sn=5 z?DHVBum!^(rD<=}G5-?|yb6ucv|DsASsr0f+UsUO5 zRC-yZS5*2LmA<6XD=NLpk@MF$jdNMWS2zjQ>XvBYaHJ9T`qJwAowy4jvw3t7hmo{+ ze$>MQ<-wt}vv{`?WU{iCiugF~%38PI@w1=<(a#qyHbBYL`V=05K?KpA9tnsK99SCkf*gSZOq$_ck!0dPJdTa zXb!a*^*Hl`gCI_V9iYVpHIYBq65B8L#LgA6%`8t4pow+r(HOl#9%HZUEKLp~A)coO z9z`!u^f`)Nr0DY$eSxBvC<4Z7aw+T!5p{FF7vy_>+~v`sY>KEIp)ib8sdUslouJpU zq?tuYTU@37EsALAVu>OG$ug=TB^@85#kud(`(|$;vYE|R^(L$9He1)%**en=hdE5g zpE>%Y66b%`7pT5&)Y!ibz72m4 z5(KvjF@A*v-D98z5Mv)0un){YY;s078&6=SdIAf80hR9&vRDYz*5Xzl+nU?lK4m{- zdF8BfuANYSLJ-C)57_tg6Z6Cx)Yyex7^~Vz1ud%Lo4L!KGeU+6uP$rcJp~H!+NsVo z;E>hUAwl)v-*BH?Z}9t0kKxZxe)Ke)Hy9o(53 zdHz(q*=foqUU_h8siVQSHoR9ShaZpqZKd^RUPDoZ+mmmx;Z5f5-1LHwWX8*)yM4Oo zUMXv%2?zMI;a$hHK^R5}s`4ljcj!sm|Ae027rj?Dyc?5?p4tO49V>U@r=| z&tnXWg~&^z(;A8-uJJ*Tv<&fOTvYMb6#WgNt>X_yO$WP`?qCo=lm}VeP)DBxU6u1X z{Vex(Xxa2=rvjG;Tn3WjDcJBjQNEY*%@z}Xg~5-TWoaIFqMP0iPU7D|d=&N`Ti)$X zcBj>l7coHSV-0>Tg8eAO4j+k^X?fQW0RRS@Sfa%%ROWsZB>m{CLV&QDG`6f@Dfa@w z6`Dw^C*p1{Z4`EbTv{UScX`VZuhOWrF7YKAhD2TD(k&0s$5F_tIu(^PqAr)NTE9j%x!Q3WbTA^?fu@ivb+3&n{-Np|r~b znu1=^6{N$}T6VCLv`d_QqH~=H;28)PH_vqd=&;v0LO{;)~MW`Dz|fXrWc^e z^K;;oj1jbBtaIa}GSzx&X4TppC;~ju+R3fLI%OwTL9^I}bq<^2q^fe2LC!5ICpA^# z6b{uB>|fDN>V;7#y0@%d(oPxxdm?_W$~9@+g2D23ZbI?sCl`tfxPMOYv&Se9k?y4V0L^xSEaDyjk;iSN`|~(RJTBsc z$Qx3o|GVN)GU@FEc?i{{$jk^}RiU53?3sji=(IeT!b#Ff1FW9a7X#(@sPouokybw`*%VBhnAMGAYp+u0K|&ZKs^IaG;%;vL7sZ7Ht!@DY9waY!Y|J|r~x zlt}~J6-a9y#L+#)9;Fq6o%SX;Az6yx)@de<4D{207AeV}qJb}zwRo?k2@w4|nuqw0 zWJfS}Y11V;kd{((gsxC@Qs;*VM+gUC>t!RW8{P8{rFtC6D&;0B&`v~IKgnm{L$TEW z(`Cd!SDUMh{xv7(#$Jk1+kLTSd`b{di?F#HjHA0f)WU=Is} zh}(&YC!`w0OFL7fuCTa4*Z?z+K|V}ij=~T~kd8W)Ue%W26F5`!?J?~YwhY>~Q;>0iS?VXKqnHXJ<84LPjbO1_(!~MnT3j*0JA&2gi9p$kzn=X!{GG z(dKZDaqX9>-YIIAw4#B~#CjKs1)x(kcm;dR{G!gGYB*Yd)?;&a}mX_y-R6smTd_WNy zhSKPz_r}(ttoFMZ7;$tQDNZ2kVcO~S^GGRwSsB)ij7We^zoRTkOfqI^3Sc6dq)pK- z&7?=LmZL;gm8D5%EB>C!odX|i4kSi44x%ItzAU7GYp@JqpZRxXH;%*)|#0SU7E72lCafQjh$Nmd=miFK-< z=s8?f(D7Aha7Yd}ARP?M1RSHSBL&7oM&<+K#3h!2Ru%9Wn47>a_)bfyg&eE&8qf#~ z%6R}Y^2~uDTFa|o8Zan{g`CuLx18aWy`|lG5t0nU6Ovia$pozz))noPVJ%It4M=EF z{a6z_Sf|Q?`?y8pmUj26n7=xabBmfXRNdg;K{H1;bS%jbtqCi1Y%X{p`jgh=raS8V zv||4MVVu(W&6}e`C&-*BI))5o>6W+GpD}ppHh5RTASMiG6h(*Lv;j?|i?)Z>FnW1n zJp=1$I0l5B$EDFasF`a0);nVY10|d$eP!Pc=9(LP-tZiEADW^tnLTRfDGPOKwvpjE zZKICI7%`{80!AIm_Z$uOSPXoe`Hlf$wU39HxI;6{a=34gGr&hjwE=GTlxHzzNs()d zq?rITTY=i5?+69Sv<7L!H;{oes&62ZC6G#JS}<}*JJ}-zy8<3^W}v?bZEF<9nRSjf z(26F?loJ{bXYd$cfTmp0RfPd?J8^O?Tim{d_Fx`v4$I&a_RT6{NcHP*Eg1!z%x87T zKX8WIaCm=o_ic>Z07@{FU>=NGg^d6w7p)yUsfwB^^ftb{kRzN%fVEN`=cyN!VT~@3 z09FtFoJ;6^OQRHU#J>A>VUfkLRq7z(W#S!*ND+Oim##wPG{ojVS}4h_N#8;M zq)`l~YMx39r~aiIB0&>rK_!9YTFTut9tJ)Fd`5|+fzLAFhI!PH4!j!b(~O8|?3y^j zLpe{#Eeu)!H%lg6@+|jK%#g`?p5{R^%sE7B6{M3>LF9f6Cd^{l%%a^6xk~8^P`+1c zwTtSe_fjG(d@#Zc{gn*}q;g@aQGbKgWX~bGuCKtu`5c02EW^)9o={KUV2jX7&__4t z^wGS#W%CvkW{_mBZB9*5?mca=_yM9VMGe#msIzD_#3|L$MHc@+(LYi|mU-!lwxN&V zrWXa3+(#r0c<03rRSGXG-arb6Hdgp*i;tDB2$4WO$`yz{^c?dC<1_V&r z5MwBP@e^ub!g0=L_Dp?bId;Qvb<=*9a zH!u!-X$N?SdlVf~!4mna@nsy2-Wdbfv7KI*~Gr&cGIXLpId zrmAlWa?Y`&2oM}Teu=Q<`}{4|B#r$TUb-w4aW5CYM2Y%fsTT0KHbb)V^-ot>Tiz~GA{0D^=_isH&Lpbxu}IEqM>6v?Sblo&FOv#~NBY!4PQ zoZVUV%#y+ys2s?YeL+=HIpoVu0j?xfy!h;#>{R{%R}MM!DXAP%ELAE_=*WtqnEAb) z+1&vMIh93C_j|8jf4zSFJ{obOVQcvP^-F)h_2Csw`)5LmKLf}$e94bcaEc#dV>S}bVce;IA@!HXu-cox>*WT24jo0sMyzaAm zrS>v+c!M|Z>+Kc)ET6f}#FoEuPeUL5rutg553wuldP|qa^_cspt#^3b^HZ_YDoNwr zIPqoq-8kr{(!MeDQeX6gxZh&Zz9qcS@1(KVkuy_Rv0{|duXb+xqUZJduf_dT#L;VU z4;V*Nn_1yKbqI8PxS}YKGQs+|*i*oO>JV%QuDPk<>bRVL1%2iH}y5d8800|ZxoC*O<=5uTU|E6)oKbj*2J3ye1@*@@Oj>tz-xuMo+_;80>4N+0qzO*S>o(aKQIopu>40l zHx$JvP)tQJbBYO`s-jd2l)9qS3zQi}naL>?^qN(a*_>j5GOsA}1)c>(Stw9WDaxq= z<&2`7DNvRaWvM_}R+QxeWkpd|3Y1kvSuIe`E6Vu-Wld4mrWA!gHO3WoVT|!h-)4WM z@9Cgc548iEKfSB}x%M4MSLstNt@F$LnZweq<~G1nOPl;kj+3+qZ{@fHd^Vlq&+cmB zJZLx-mt->wp3i}{nA1o)v*~I69B5~9+ETh4t{j{_)c!yh!*uyzm4E(F?;8qQKg8QI(f4%hUV?@LWEG#<vG|k zIdJ>9aI&%;i(nM@A1>jR$y$Hda}%#Oi2Nitm;t3cbbCR+i$?nWFb9701J&StFmAXu zZn!GVBkrqU;S!6<=V9t6Bn>;aS_GH zf+^h!x?frw&BeoXFibac+v;|D{2^Jc(R|)8h+waMu9i8nxxYE=ZqSlPPSB^0w}Zal z^U|P`jFvOXbZhCv9gG@TTL(tF=MALM8FEjSI)mY@mLbh-IkK#VmnQe`1ReMop6(3% z?)T6A{)acd{})$VS7bRE4tyak7Xk}X*Ojv{w2>!Bio%13LSv)rrv-HW2&=c~%2uPX#)nge1Mh5$r-`*5B(8VI3nC8!5-lQCwxZZeUwTnI z3ry6BWasJw7HP$KtRU^_ys|PLyq<3|4SI^X?`BY0RR|)C#E-VbGZ^-PE~~DK^Gy=h zeV~1EYr_jV+t|L}@A?~eV{tnfcpZOZa~MQ?byHtp zqm?J)j`hjiDD{$Vha!*hk6`*YaKDvCu^izeLXxC>hLfoKHe**<3e}mwgR2-Z2SBOU6EfgotIn}fO=bUfS zuqVxV6%E75oEJ~n=kY6aB;KP-JC=0vWgryqDG#l~Loa+PSh>KvPDriBN8#=taU*4m&tR9@(#X2bf1biNapw4Ch1)s)jC%#6 zw+^1&Rf$ddIVe5kd+;1)_&mP#Lv7a%H{9oWP1P?Tr6L(wNRmh5 zyO#H+`URvunS@uvm&U7BBh}PMU%*H;8cBtX&Fc^8Mj{MSZi_tG5nWOy4MegA2^qgG zWcS`BTv|QULOrBQN_8nU4$2BPbD9bLqBhd4thSVnXwVTotagn*WbZRvU0rw>wq;9W zKpC`!GEmn~|3K`D5DnITWVGs@BIAJkj{B z8lN(FH^XEfn~z5uC*0XM_u}g>V_q*xUI8#3w^f!1HHia~K!R6QhXDlVarC5-XJsX1t&Q+xE6DAcK4$F0{=-24p-J@lbz*!K1zenZ^-%$fP$KA1+f z=x(R7d^h0fwzSb0M?)2@t2pI#VyY+PmB)H-4hI;>mowz6(|9n>OvPp5EhAS&BubuA zw5j+VD!xX=dsNUHmiPq}Dy5(lL!}hrD@0c=`K#23b$J}g$8Qs2ZBatx23-6iq0MPl zM%kPAB@kQnEYx#}p^j()DNH6&o}A70I<##!@!yx#Yybjlg&nFe1Uok~ zR#`ui0%ZxMN`H>4;ICm-_wXfs6v`SQLZ*n^m_+57U8-uPEoLQ{}$yz^7 z*QQtF5~jwZ=ps%wPE(|^e3->{IGWBIqm^ld{6yc;nfy?nXt~uYXE*E%7+tA;7Npa~ zfSp!p!ZEw?4Kz`Jnk=9&^*J0?u|&wXeuDe>IhVfGq-?)Ws_aVJb$c;~O9WhZ$s~^TUfzlj7(Y*LN6{J*x7AYHfW~m&KTTs5Yh()D4O+e&a5-KdM&YJHv zOU;s1vT6<&)Oti0D1snYrF7v6fE4kcd{jCg=I9xK@%raZojhj#I8j4lm&^(ri!)+~*l7xW(iO2`{P&2<~f zF1|}`5G1v_ZkBSbitiBBR`ac^!DtJz-uLcM0+Fnv$lf1jyMFfmKc$n_D?g(gj66^8 zUw7#xMm>b7OHO&jWSk1|)0*VDqpahXrwM13IO*JEHCx*IaUyDjV{=e&@&MvngtPu2 z^K9pPNB=*tehklEDW2dOo9EoTWoDnwRmn^I2|-f@SCA62^>U_$uh8{R=%}F`5c7hv VECh`sCxRWO{hrM&)7Fj0{|l9Y2e1GD From ea112e4d9ac4f3a7aed94b37802251b306cab63d Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 20:25:13 +0900 Subject: [PATCH 10/12] fix typo --- .cspell.json | 10 ++++++++++ .../mppi_controller/mppi_controller/MPPI.py | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/.cspell.json b/.cspell.json index 7fef0397..b31b79d0 100644 --- a/.cspell.json +++ b/.cspell.json @@ -111,6 +111,16 @@ "MPPI", "Savitzky", "Golay", + "Vandermonde", + "michikuni", + "eguchi", + "vander", + "pinv", + "dtheta", + "cind", + "dind", + "ncourse", + "argmin", "reparameting", "tempature", "rsample", diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py index d0fa4af7..90f52793 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -323,7 +323,7 @@ def forward( dim=0, ) - # appply sg filter for each control dimension + # apply sg filter for each control dimension filtered_action_seq = torch.zeros_like( prolonged_action_seq, device=self._device, dtype=self._dtype ) From 202e565327ed579b20160b035e07103ba03a13c2 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 20:26:19 +0900 Subject: [PATCH 11/12] add some spells --- .cspell.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.cspell.json b/.cspell.json index b31b79d0..7978a84c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -110,6 +110,7 @@ "Kohei", "MPPI", "Savitzky", + "satitzky", "Golay", "Vandermonde", "michikuni", From 4b0f44a642c3b384818f06808b475343e6ce8684 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 21:47:34 +0900 Subject: [PATCH 12/12] mppi + pure pursuit --- .../config/planning/costmap_generator.param.yaml | 2 +- .../booars_launch/launch/components/control.launch.xml | 7 ++++--- .../mppi_controller/config/mppi_controller.param.yaml | 8 ++++---- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 761d16e7..c5c54ba6 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -15,4 +15,4 @@ predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" - distance_threshold: 1.0 + distance_threshold: 1.2 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index f23f0812..d39ec7b6 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -5,12 +5,13 @@ - - + + - + + diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml index 15fa92cd..8f0d35a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -5,16 +5,16 @@ num_samples : 3000 u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad) u_max : [2.0, 0.25] - sigmas : [0.5, 0.1] # sample range + sigmas : [0.5, 0.15] # sample range lambda : 1.0 auto_lambda : false # reference path DL : 0.1 - lookahead_distance : 3.0 + lookahead_distance : 0.3 reference_path_interval : 0.85 # cost weights - Qc : 2.0 - Ql : 3.0 + Qc : 20.0 + Ql : 1.0 Qv : 2.0 Qo : 10000.0 Qin : 0.01