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 f670c1e7..e341d1c8 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 @@ -4,7 +4,7 @@ map_frame_id: "map" costmap_center_frame_id: "base_link" costmap_width: 50.0 # [m] - costmap_resolution: 0.2 # [m/cell] + costmap_resolution: 0.1 # [m/cell] multi_layered_costmap: layers: - "cached_lanelet_layer" @@ -13,7 +13,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" - inflation_radius: 1.8 # [m] + inflation_radius: 1.4 # [m] cached_costmap: min_x: 89607.0 # [m] max_x: 89687.0 # [m] diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml index 5b053b66..7f21479c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml @@ -24,8 +24,9 @@ pit_goal.orientation.z: -0.8788172006607056 pit_goal.orientation.w: -0.47715866565704346 # ゴールまでの距離がこの値以下になると次のゴールを配信する - goal_range: 30.0 + goal_range: 10.0 # ピットインを有効化するか enable_pit: true # このしきい値以下になるとピットイン - pit_in_threshold: 1000 + # pit_in_threshold: 100 + pit_in_threshold: 700 \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml index ca77d864..def6c69d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -2,10 +2,10 @@ ros__parameters: # mppi horizon : 25 - num_samples : 3000 - u_min : [-2.0, -0.35] # accel(m/s2), steer angle(rad) - u_max : [2.0, 0.35] - sigmas : [0.5, 0.25] # sample range + num_samples : 4000 + u_min : [-4.0, -0.35] # accel(m/s2), steer angle(rad) + u_max : [3.0, 0.35] + sigmas : [2.0, 0.35] # sample range lambda : 1.0 auto_lambda : false # reference path @@ -13,14 +13,14 @@ lookahead_distance : 0.1 reference_path_interval : 0.8 # cost weights - Qc : 20.0 + Qc : 10.0 Ql : 1.0 - Qv : 2.0 - Qo : 10000.0 + Qv : 4.0 + Qo : 1000.0 Qin : 0.01 Qdin : 0.5 # model param delta_t : 0.1 - vehicle_L : 1.0 - V_MAX : 8.0 + vehicle_L : 1.08 + V_MAX : 8.33 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 350d33c5..d5d98a9c 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 @@ -12,11 +12,11 @@ - + - - + + 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 9bd276b8..c964c2b8 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 @@ -99,7 +99,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm index 28d60946..3754ea01 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm @@ -1,4850 +1,4042 @@ - - - + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - + - - - - + + + - - + - + - - + - - + - + - - - - + + + - - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + @@ -5693,7 +4885,7 @@ - + @@ -5703,7 +4895,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm new file mode 100644 index 00000000..28d60946 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm @@ -0,0 +1,5710 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp index 3257394a..604d9a53 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp @@ -51,8 +51,11 @@ void PathToTrajectory::callback(const PathWithLaneId::SharedPtr msg) { const double t= traj_width_/v; v+=dec_mpss*t; v=std::max(0.0,v); - trajectory.points.at(trajectory.points.size()-offset_index+i).longitudinal_velocity_mps - =std::min(float(v), trajectory.points.at(trajectory.points.size()-offset_index+i).longitudinal_velocity_mps); + int index = trajectory.points.size() - offset_index + i; + if (index >= 0 && index < trajectory.points.size()) { + trajectory.points.at(index).longitudinal_velocity_mps + = std::min(float(v), trajectory.points.at(index).longitudinal_velocity_mps); + } } pub_->publish(trajectory); } diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index b4406f6a..20200939 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -78,18 +78,9 @@ void SimplePurePursuit::onTimer() // get closest trajectory point from current position TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); - // calc longitudinal speed and acceleration - double target_longitudinal_vel = - use_external_target_vel_ ? external_target_vel_ : closet_traj_point.longitudinal_velocity_mps; - double current_longitudinal_vel = odometry_->twist.twist.linear.x; - - cmd.longitudinal.speed = target_longitudinal_vel; - cmd.longitudinal.acceleration = - speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); - // calc lateral control //// calc lookahead distance - double lookahead_distance = lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_; + double lookahead_distance = lookahead_gain_ * closet_traj_point.longitudinal_velocity_mps + lookahead_min_distance_; //// calc center coordinate of rear wheel double rear_x = odometry_->pose.pose.position.x - wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z); @@ -107,6 +98,15 @@ void SimplePurePursuit::onTimer() } double lookahead_point_x = lookahead_point_itr->pose.position.x; double lookahead_point_y = lookahead_point_itr->pose.position.y; + + // calc longitudinal speed and acceleration + double target_longitudinal_vel = + use_external_target_vel_ ? external_target_vel_ : lookahead_point_itr->longitudinal_velocity_mps; + double current_longitudinal_vel = odometry_->twist.twist.linear.x; + + cmd.longitudinal.speed = target_longitudinal_vel; + cmd.longitudinal.acceleration = + speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); { // publish lookahead point marker auto marker_msg = Marker(); @@ -133,10 +133,15 @@ void SimplePurePursuit::onTimer() mkr_cmd_->publish(marker_msg); } // calc steering angle for lateral control - double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - - tf2::getYaw(odometry_->pose.pose.orientation); - cmd.lateral.steering_tire_angle = - steering_tire_angle_gain_ * std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); + if (std::hypot(lookahead_point_x - rear_x, lookahead_point_y - rear_y) < lookahead_min_distance_) { + cmd.lateral.steering_tire_angle = 0.0; + } else { + double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - + tf2::getYaw(odometry_->pose.pose.orientation); + cmd.lateral.steering_tire_angle = + steering_tire_angle_gain_ * + std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); + } } pub_cmd_->publish(cmd); cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_; 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 ba3f9b7b..71b99a61 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 @@ -8,11 +8,10 @@ Panels: - /Planning1/ScenarioPlanning1/ScenarioTrajectory1 - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 - /Planning1/Costmap1/Map1 - - /Planning1/MPPI1/Trajectory1 - - /Planning1/MPPI1/ReferenceTrajectory1 + - /Planning1/MPPI1 - /Planning1/MPPI1/ReferenceTrajectory1/View Path1 Splitter Ratio: 0.557669460773468 - Tree Height: 158 + Tree Height: 140 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -725,12 +724,7 @@ Visualization Manager: Color: 119; 11; 32 Name: PredictedObjects Namespaces: - acceleration: true - label: true - shape: true - twist: true - uuid: true - velocity: true + {} PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -833,11 +827,7 @@ Visualization Manager: Enabled: true Name: RouteArea Namespaces: - goal_lanelets: true - lane_start_bound: false - left_lane_bound: false - right_lane_bound: false - route_lanelets: true + {} Topic: Depth: 5 Durability Policy: Transient Local @@ -1240,8 +1230,7 @@ Visualization Manager: Enabled: true Name: Bound Namespaces: - left_bound: true - right_bound: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2012,7 +2001,7 @@ Visualization Manager: Value: true - Class: rviz_plugins/Trajectory Color Border Vel Max: 3 - Enabled: true + Enabled: false Name: ReferenceTrajectory Topic: Depth: 5 @@ -2021,7 +2010,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/debug/path - Value: true + Value: false View Footprint: Alpha: 1 Color: 230; 230; 50 @@ -2128,7 +2117,7 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2311,12 +2300,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1043 + Height: 963 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000297000003e4fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000014000000d9000000c700fffffffc000000f3000000cc000000bb0100001afa000000000100000002fb0000000a0056006900650077007301000000000000033c0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000008c00fffffffb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c01000001c50000017d0000015e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007100fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065000000038a0000010b0000002800fffffffb0000002a004100750074006f0077006100720065004400610074006500540069006d006500500061006e0065006c0100000348000000450000003f00fffffffb00000034004100750074006f007700610072006500530063007200650065006e004300610070007400750072006500500061006e0065006c0100000393000000650000005d00ffffff000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000435000003e400000004000000040000000800000008fc0000000100000000000000010000000a0054006f006f006c00730300000000ffffffff0000000000000000 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -2325,6 +2314,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 32 + Width: 1440 + X: 240 + Y: 112