From ac046e7bac716e21d1dd7d10981a7de37bc5504d Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 3 Oct 2024 23:09:06 +0900 Subject: [PATCH] feat(mission_planning): add pit-in control (rebased) (#52) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * pit in path * chore: Update goal_pose_setter configuration to enable pit and adjust threshold * ignore result json * add pit in speed control * add spell * fixed bug * add mission planner param * mission planner param 調整 * pitin ok * param fileをbooars_launchに移行 * fix typo * parameter tuning * parameter update * add .gitignore for .vscode Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 Co-authored-by: hrjp Co-authored-by: tamago117 --- .cspell.json | 3 +- .gitignore | 1 + aichallenge/.gitignore | 2 + aichallenge/workspace/.gitignore | 3 +- .../launch/aichallenge_submit.launch.xml | 2 +- .../planning/costmap_generator.param.yaml | 6 +- .../config/planning/goal_pose.param.yaml | 32 + .../planning/mission_planner.param.yaml | 13 + .../planning/mppi_controller.param.yaml | 20 +- .../launch/components/control.launch.xml | 6 +- .../launch/components/planning.launch.xml | 10 +- .../booars_launch/map/lanelet2_map_opt.osm | 2512 +++----- .../map/lanelet2_map_opt_pre.osm | 5710 +++++++++++++++++ .../config/default_goal_pose.param.yaml | 11 + .../goal_pose_setter/package.xml | 1 + .../src/goal_pose_setter_node.cpp | 89 +- .../src/goal_pose_setter_node.hpp | 19 + .../path_to_trajectory/path_to_trajectory.hpp | 4 + .../src/path_to_trajectory.cpp | 26 + .../src/simple_pure_pursuit.cpp | 33 +- .../config/autoware.rviz | 37 +- 21 files changed, 6815 insertions(+), 1725 deletions(-) create mode 100644 .gitignore create mode 100644 aichallenge/.gitignore create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm diff --git a/.cspell.json b/.cspell.json index 66c0f12b..df7e9be3 100644 --- a/.cspell.json +++ b/.cspell.json @@ -137,6 +137,7 @@ "tempature", "rsample", "coeffs", - "softplus" + "softplus", + "mpss" ] } diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..600d2d33 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode \ No newline at end of file diff --git a/aichallenge/.gitignore b/aichallenge/.gitignore new file mode 100644 index 00000000..71c36e20 --- /dev/null +++ b/aichallenge/.gitignore @@ -0,0 +1,2 @@ +capture +result-details.json \ No newline at end of file diff --git a/aichallenge/workspace/.gitignore b/aichallenge/workspace/.gitignore index ab2524be..fca9e03b 100644 --- a/aichallenge/workspace/.gitignore +++ b/aichallenge/workspace/.gitignore @@ -3,4 +3,5 @@ /log .vscode -*.code-workspace \ No newline at end of file +*.code-workspace +result-details.json diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml index da639962..ec2952a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml @@ -22,6 +22,6 @@ - + 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..283e0a7f 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] @@ -23,4 +23,4 @@ predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" - distance_threshold: 1.3 + distance_threshold: 1.2 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 new file mode 100644 index 00000000..7f21479c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml @@ -0,0 +1,32 @@ +/**: + ros__parameters: + goal.position.x: 89653.7 + goal.position.y: 43122.5 + goal.position.z: 0.0 + goal.orientation.x: 0.0 + goal.orientation.y: 0.0 + goal.orientation.z: -0.971732 + goal.orientation.w: 0.236088 + + half_goal.position.x: 89657.0 + half_goal.position.y: 43175.0 + half_goal.position.z: -28.0 + half_goal.orientation.x: 0.0 + half_goal.orientation.y: 0.0 + half_goal.orientation.z: -0.9 + half_goal.orientation.w: 0.25 + + pit_goal.position.x: 89626.3671875 + pit_goal.position.y: 43134.921875 + pit_goal.position.z: 42.10000228881836 + pit_goal.orientation.x: 0.0 + pit_goal.orientation.y: 0.0 + pit_goal.orientation.z: -0.8788172006607056 + pit_goal.orientation.w: -0.47715866565704346 + # ゴールまでの距離がこの値以下になると次のゴールを配信する + goal_range: 10.0 + # ピットインを有効化するか + enable_pit: true + # このしきい値以下になるとピットイン + # 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/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml new file mode 100644 index 00000000..a0aa4f74 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + map_frame: map + arrival_check_angle_deg: 180.0 + arrival_check_distance: 30.0 + arrival_check_duration: 1.0 + goal_angle_threshold_deg: 90.0 + enable_correct_goal_pose: false + reroute_time_threshold: 10.0 + minimum_reroute_length: 30.0 + consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. + check_footprint_inside_lanes: false + allow_reroute_in_autonomous_mode: true \ 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..35ba01cc 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 + Qdin : 0.0 # 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 c1deb358..a1f3cb0c 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 @@ -14,7 +14,7 @@ - + @@ -98,6 +98,14 @@ + + + + + + + + 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/goal_pose_setter/config/default_goal_pose.param.yaml b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml index 32dae394..87732d8f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml @@ -16,4 +16,15 @@ half_goal.orientation.z: -0.9 half_goal.orientation.w: 0.25 + pit_goal.position.x: 89626.3671875 + pit_goal.position.y: 43134.921875 + pit_goal.position.z: 42.10000228881836 + pit_goal.orientation.x: 0.0 + pit_goal.orientation.y: 0.0 + pit_goal.orientation.z: -0.8788172006607056 + pit_goal.orientation.w: -0.47715866565704346 + goal_range: 10.0 + + enable_pit: true + pit_in_threshold: 20 diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml index cca3510f..b508fbea 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml @@ -15,6 +15,7 @@ std_srvs nav2_msgs tier4_autoware_utils + std_msgs ament_cmake diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp index bc2a50bf..c56d90f1 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp @@ -12,6 +12,33 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") odometry_subscriber_ = this->create_subscription( "/localization/kinematic_state", 1, std::bind(&GoalPosePublisher::odometry_callback, this, std::placeholders::_1)); + + pit_position_subscriber_ = this->create_subscription( + "/aichallenge/pitstop/area", 1, + [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) { + pit_position_.position.x = msg->data[0]; + pit_position_.position.y = msg->data[1]; + pit_position_.position.z = msg->data[2]; + pit_position_.orientation.x = msg->data[3]; + pit_position_.orientation.y = msg->data[4]; + pit_position_.orientation.z = msg->data[5]; + pit_position_.orientation.w = msg->data[6]; + }); + + pit_condition_subscriber_ = this->create_subscription( + "/aichallenge/pitstop/condition", 1, + [this](const std_msgs::msg::Int32::SharedPtr msg) { + pit_condition_ = msg->data; + }); + + pit_stop_time_subscriber_ = this->create_subscription( + "/aichallenge/pitstop/status", 1, + [this](const std_msgs::msg::Float32::SharedPtr msg) { + pit_stop_time_ = msg->data; + }); + + + timer_ = this->create_wall_timer( std::chrono::milliseconds(300), std::bind(&GoalPosePublisher::on_timer, this)); @@ -32,8 +59,19 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") this->declare_parameter("half_goal.orientation.z", -0.9); this->declare_parameter("half_goal.orientation.w", 0.25); + this->declare_parameter("pit_goal.position.x", 89626.3671875); + this->declare_parameter("pit_goal.position.y", 43134.921875); + this->declare_parameter("pit_goal.position.z", 42.10000228881836); + this->declare_parameter("pit_goal.orientation.x", 0.0); + this->declare_parameter("pit_goal.orientation.y", 0.0); + this->declare_parameter("pit_goal.orientation.z", -0.8788172006607056); + this->declare_parameter("pit_goal.orientation.w", -0.47715866565704346); + this->declare_parameter("goal_range", 10.0); + this->declare_parameter("enable_pit", true); + this->declare_parameter("pit_in_threshold", 1000); + goal_position_.position.x = this->get_parameter("goal.position.x").as_double(); goal_position_.position.y = this->get_parameter("goal.position.y").as_double(); goal_position_.position.z = this->get_parameter("goal.position.z").as_double(); @@ -51,7 +89,19 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") half_goal_position_.orientation.z = this->get_parameter("half_goal.orientation.z").as_double(); half_goal_position_.orientation.w = this->get_parameter("half_goal.orientation.w").as_double(); + pit_position_.position.x = this->get_parameter("pit_goal.position.x").as_double(); + pit_position_.position.y = this->get_parameter("pit_goal.position.y").as_double(); + pit_position_.position.z = this->get_parameter("pit_goal.position.z").as_double(); + pit_position_.orientation.x = this->get_parameter("pit_goal.orientation.x").as_double(); + pit_position_.orientation.y = this->get_parameter("pit_goal.orientation.y").as_double(); + pit_position_.orientation.z = this->get_parameter("pit_goal.orientation.z").as_double(); + pit_position_.orientation.w = this->get_parameter("pit_goal.orientation.w").as_double(); + + goal_range_ = this->get_parameter("goal_range").as_double(); + + enable_pit_ = this->get_parameter("enable_pit").as_bool(); + pit_in_threshold_ = this->get_parameter("pit_in_threshold").as_int(); } void GoalPosePublisher::on_timer() @@ -107,22 +157,47 @@ void GoalPosePublisher::odometry_callback(const nav_msgs::msg::Odometry::SharedP { if (!is_started_) return; - + //RCLCPP_INFO(this->get_logger(), "%lf",pit_position_.position.x ); // Publish half goal pose for loop - if(half_goal_pose_published_ == false && - tier4_autoware_utils::calcDistance2d(msg->pose.pose, goal_position_) < goal_range_) + if(half_goal_pose_published_ == true && + tier4_autoware_utils::calcDistance2d(msg->pose.pose, goal_position_) < goal_range_ + && pit_in_flag_ == false) { auto goal_pose = std::make_shared(); goal_pose->header.stamp = this->get_clock()->now(); goal_pose->header.frame_id = "map"; - goal_pose->pose = half_goal_position_; + if(pit_condition_ > pit_in_threshold_ && enable_pit_ == true){ + goal_pose->pose = pit_position_; + pit_in_flag_ = true; + } + else{ + goal_pose->pose = half_goal_position_; + half_goal_pose_published_ = false; + } + + goal_publisher_->publish(*goal_pose); + RCLCPP_INFO(this->get_logger(), "Publishing half goal pose for loop"); + + } + if(pit_in_flag_ == true && + tier4_autoware_utils::calcDistance2d(msg->pose.pose, pit_position_) < goal_range_ + && pit_stop_time_ > 3.1) + { + auto goal_pose = std::make_shared(); + goal_pose->header.stamp = this->get_clock()->now(); + goal_pose->header.frame_id = "map"; + goal_pose->pose = half_goal_position_; + half_goal_pose_published_ = false; + pit_in_flag_ = false; + goal_publisher_->publish(*goal_pose); RCLCPP_INFO(this->get_logger(), "Publishing half goal pose for loop"); - half_goal_pose_published_ = true; + } + // Publish goal pose for loop - if (half_goal_pose_published_ == true && + if (half_goal_pose_published_ == false && tier4_autoware_utils::calcDistance2d(msg->pose.pose, half_goal_position_) < goal_range_) { auto goal_pose = std::make_shared(); @@ -132,7 +207,7 @@ void GoalPosePublisher::odometry_callback(const nav_msgs::msg::Odometry::SharedP goal_publisher_->publish(*goal_pose); RCLCPP_INFO(this->get_logger(), "Publishing goal pose for loop"); - half_goal_pose_published_ = false; + half_goal_pose_published_ = true; } } diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp index 0f4b3d20..a712d9e8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp @@ -17,6 +17,10 @@ #include +#include +#include +#include + #include #include #include @@ -24,6 +28,8 @@ #include #include + + class GoalPosePublisher : public rclcpp::Node { public: @@ -38,6 +44,11 @@ class GoalPosePublisher : public rclcpp::Node rclcpp::Publisher::SharedPtr goal_publisher_; rclcpp::Subscription::SharedPtr route_state_subscriber_; rclcpp::Subscription::SharedPtr odometry_subscriber_; + + rclcpp::Subscription::SharedPtr pit_position_subscriber_; + rclcpp::Subscription::SharedPtr pit_condition_subscriber_; + rclcpp::Subscription::SharedPtr pit_stop_time_subscriber_; + rclcpp::TimerBase::SharedPtr timer_; bool stop_initializing_pose_ = false; bool stop_streaming_goal_pose_ = false; @@ -47,6 +58,14 @@ class GoalPosePublisher : public rclcpp::Node float goal_range_; geometry_msgs::msg::Pose goal_position_; geometry_msgs::msg::Pose half_goal_position_; + + geometry_msgs::msg::Pose pit_position_; + int pit_condition_; + float pit_stop_time_; + bool pit_in_flag_ = false; + + bool enable_pit_ = true; + int pit_in_threshold_ = 1000; }; #endif // GOAL_POSE_SETTER_NODE_ diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp index c24836f8..600fd08d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp @@ -32,6 +32,10 @@ class PathToTrajectory : public rclcpp::Node { void callback(const PathWithLaneId::SharedPtr msg); rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; + double deceleration_; + double stop_offset_; + double max_speed_; + double traj_width_; }; #endif // PATH_TO_TRAJECTORY__PATH_TO_TRAJECTORY_HPP_ 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 5e078e56..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 @@ -20,6 +20,16 @@ PathToTrajectory::PathToTrajectory() : Node("path_to_trajectory_node") { pub_ = this->create_publisher("output", 1); sub_ = this->create_subscription( "input", 1, std::bind(&PathToTrajectory::callback, this, _1)); + + this->declare_parameter("deceleration", -10.0); + this->declare_parameter("stop_offset", 3.0); + this->declare_parameter("max_speed", 30.0); + this->declare_parameter("traj_width", 1.0); + + deceleration_ = this->get_parameter("deceleration").as_double(); + stop_offset_ = this->get_parameter("stop_offset").as_double(); + max_speed_ = this->get_parameter("max_speed").as_double(); + traj_width_ = this->get_parameter("traj_width").as_double(); } void PathToTrajectory::callback(const PathWithLaneId::SharedPtr msg) { @@ -31,6 +41,22 @@ void PathToTrajectory::callback(const PathWithLaneId::SharedPtr msg) { trajectory_point.longitudinal_velocity_mps = path_point_with_lane_id.point.longitudinal_velocity_mps; trajectory.points.emplace_back(std::move(trajectory_point)); } + const double stop_time= max_speed_/std::abs(deceleration_); + const double dec_mpss=deceleration_/3.6; + const double speed_mps=max_speed_/3.6; + const double stop_dis=0.5*dec_mpss*stop_time*stop_time+speed_mps*stop_time+stop_offset_; + const int offset_index=stop_dis/traj_width_; + double v=speed_mps; + for(int i=0 ; i= 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: 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 + 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