changes for experiment (nice path sampler tuning for lane changes)

maxime-clem · maxime-clem · commit 3557bbbd6f95 · 2024-08-01T11:33:02.000+09:00
Signed-off-by: Maxime CLEMENT &lt;maxime.clement@tier4.jp&gt;
diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
@@ -360,7 +360,7 @@ def launch_setup(context, *args, **kwargs):
             package="rclcpp_components",
             executable=LaunchConfiguration("container_executable"),
             composable_node_descriptions=[
-                lane_departure_component,
+                # lane_departure_component,
                 shift_decider_component,
                 vehicle_cmd_gate_component,
                 autoware_operation_mode_transition_manager_component,
@@ -414,8 +414,8 @@ def launch_setup(context, *args, **kwargs):
             external_cmd_selector_loader,
             external_cmd_converter_loader,
             obstacle_collision_checker_loader,
-            autonomous_emergency_braking_loader,
-            predicted_path_checker_loader,
+            # autonomous_emergency_braking_loader,
+            # predicted_path_checker_loader,
             control_evaluator_loader,
         ]
     )
diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -14,12 +14,12 @@
   <group>
     <push-ros-namespace namespace="planning"/>
     <!-- mission planning module -->
-    <group>
+    <!-- <group>
       <push-ros-namespace namespace="mission_planning"/>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml">
         <arg name="mission_planner_param_path" value="$(var mission_planner_param_path)"/>
       </include>
-    </group>
+    </group> -->
 
     <!-- scenario planning module -->
     <group>
@@ -31,22 +31,22 @@
     </group>
 
     <!-- planning validator -->
-    <group>
+    <!-- <group>
       <include file="$(find-pkg-share autoware_planning_validator)/launch/planning_validator.launch.xml">
         <arg name="input_trajectory" value="/planning/scenario_planning/velocity_smoother/trajectory"/>
         <arg name="output_trajectory" value="/planning/scenario_planning/trajectory"/>
         <arg name="planning_validator_param_path" value="$(var planning_validator_param_path)"/>
       </include>
-    </group>
+    </group> -->
 
     <!-- planning evaluator -->
-    <group>
+    <!-- <group>
       <include file="$(find-pkg-share autoware_planning_evaluator)/launch/planning_evaluator.launch.xml"/>
-    </group>
+    </group> -->
 
     <!-- mission remaining distance and time calculator -->
-    <group>
+    <!-- <group>
       <include file="$(find-pkg-share autoware_remaining_distance_time_calculator)/launch/remaining_distance_time_calculator.launch.xml"/>
-    </group>
+    </group> -->
   </group>
 </launch>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -6,18 +6,17 @@
   <group>
     <push-ros-namespace namespace="lane_driving"/>
     <!-- behavior planning module -->
-    <group>
+    <!--<group>
       <push-ros-namespace namespace="behavior_planning"/>
       <group>
         <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml">
           <arg name="container_type" value="component_container_mt"/>
           <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
           <arg name="is_simulation" value="$(var is_simulation)"/>
-          <!-- This condition should be true if run_out module is enabled and its detection method is Points -->
           <arg name="launch_compare_map_pipeline" value="false"/>
         </include>
       </group>
-    </group>
+    </group>-->
 
     <!-- motion planning module -->
     <group>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -72,7 +72,7 @@
           <!-- topic remap -->
           <remap from="~/input/path" to="path_smoother/path"/>
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-          <remap from="~/output/path" to="path_optimizer/trajectory"/>
+          <remap from="~/output/path" to="/planning/scenario_planning/scenario_selector/trajectory"/>
           <!-- params -->
           <param from="$(var common_param_path)"/>
           <param from="$(var vehicle_param_file)"/>
@@ -90,7 +90,7 @@
           <!-- topic remap -->
           <remap from="~/input/path" to="path_smoother/path"/>
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-          <remap from="~/output/path" to="path_optimizer/trajectory"/>
+          <remap from="~/output/path" to="/planning/scenario_planning/scenario_selector/trajectory"/>
           <!-- params -->
           <param from="$(var common_param_path)"/>
           <param from="$(var vehicle_param_file)"/>
@@ -107,133 +107,11 @@
         <composable_node pkg="autoware_planning_topic_converter" plugin="autoware::planning_topic_converter::PathToTrajectory" name="path_to_trajectory_converter" namespace="">
           <!-- params -->
           <param name="input_topic" value="path_smoother/path"/>
-          <param name="output_topic" value="path_optimizer/trajectory"/>
+          <param name="output_topic" value="/planning/scenario_planning/scenario_selector/trajectory"/>
           <!-- composable node config -->
           <extra_arg name="use_intra_process_comms" value="false"/>
         </composable_node>
       </load_composable_node>
     </group>
   </group>
-
-  <!-- plan slowdown or stops on the final trajectory -->
-  <group>
-    <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-      <composable_node pkg="autoware_motion_velocity_planner_node" plugin="autoware::motion_velocity_planner::MotionVelocityPlannerNode" name="motion_velocity_planner" namespace="">
-        <!-- topic remap -->
-        <remap from="~/input/trajectory" to="path_optimizer/trajectory"/>
-        <remap from="~/input/vector_map" to="/map/vector_map"/>
-        <remap from="~/input/vehicle_odometry" to="/localization/kinematic_state"/>
-        <remap from="~/input/accel" to="/localization/acceleration"/>
-        <remap from="~/input/dynamic_objects" to="/perception/object_recognition/objects"/>
-        <remap from="~/input/no_ground_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
-        <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
-        <remap from="~/input/virtual_traffic_light_states" to="/perception/virtual_traffic_light_states"/>
-        <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
-        <remap from="~/output/trajectory" to="motion_velocity_planner/trajectory"/>
-        <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-        <remap from="~/output/velocity_factors" to="/planning/velocity_factors/motion_velocity_planner"/>
-        <!-- params -->
-        <param name="launch_modules" value="$(var motion_velocity_planner_launch_modules)"/>
-        <param from="$(var common_param_path)"/>
-        <param from="$(var vehicle_param_file)"/>
-        <param from="$(var nearest_search_param_path)"/>
-        <param from="$(var velocity_smoother_param_path)"/>
-        <param from="$(var motion_velocity_planner_velocity_smoother_type_param_path)"/>
-        <param from="$(var motion_velocity_planner_param_path)"/>
-        <param from="$(var motion_velocity_planner_dynamic_obstacle_stop_module_param_path)"/>
-        <param from="$(var motion_velocity_planner_out_of_lane_module_param_path)"/>
-        <param from="$(var motion_velocity_planner_obstacle_velocity_limiter_param_path)"/>
-        <!-- <param from="$(var motion_velocity_planner_template_param_path)"/> -->
-        <!-- composable node config -->
-        <extra_arg name="use_intra_process_comms" value="false"/>
-      </composable_node>
-    </load_composable_node>
-  </group>
-
-  <!-- obstacle stop, adaptive cruise -->
-  <group>
-    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner'&quot;)">
-      <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-        <composable_node pkg="autoware_obstacle_cruise_planner" plugin="autoware::motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
-          <!-- topic remap -->
-          <remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
-          <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-          <remap from="~/input/acceleration" to="/localization/acceleration"/>
-          <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
-          <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
-          <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-          <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
-          <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
-          <!-- params -->
-          <param from="$(var common_param_path)"/>
-          <param from="$(var vehicle_param_file)"/>
-          <param from="$(var nearest_search_param_path)"/>
-          <param from="$(var obstacle_cruise_planner_param_path)"/>
-          <!-- composable node config -->
-          <extra_arg name="use_intra_process_comms" value="false"/>
-        </composable_node>
-      </load_composable_node>
-    </group>
-
-    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_stop_planner'&quot;)">
-      <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-        <composable_node pkg="obstacle_stop_planner" plugin="motion_planning::ObstacleStopPlannerNode" name="obstacle_stop_planner" namespace="">
-          <!-- topic remap -->
-          <remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
-          <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-          <remap from="~/input/acceleration" to="/localization/acceleration"/>
-          <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
-          <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
-          <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
-          <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-          <remap from="~/output/max_velocity" to="/planning/scenario_planning/max_velocity_candidates"/>
-          <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit"/>
-          <!-- params -->
-          <param from="$(var common_param_path)"/>
-          <param from="$(var vehicle_param_file)"/>
-          <param from="$(var nearest_search_param_path)"/>
-          <param from="$(var obstacle_stop_planner_param_path)"/>
-          <param from="$(var obstacle_stop_planner_acc_param_path)"/>
-          <!-- composable node config -->
-          <extra_arg name="use_intra_process_comms" value="false"/>
-        </composable_node>
-      </load_composable_node>
-    </group>
-
-    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'none'&quot;)">
-      <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-        <composable_node pkg="topic_tools" plugin="topic_tools::RelayNode" name="obstacle_stop_relay" namespace="">
-          <!-- params -->
-          <param name="input_topic" value="motion_velocity_planner/trajectory"/>
-          <param name="output_topic" value="$(var interface_output_topic)"/>
-          <param name="type" value="autoware_planning_msgs/msg/Trajectory"/>
-          <!-- composable node config -->
-          <extra_arg name="use_intra_process_comms" value="false"/>
-        </composable_node>
-      </load_composable_node>
-    </group>
-  </group>
-
-  <!-- surround obstacle check -->
-  <group if="$(var launch_surround_obstacle_checker)">
-    <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-      <composable_node pkg="autoware_surround_obstacle_checker" plugin="autoware::surround_obstacle_checker::SurroundObstacleCheckerNode" name="surround_obstacle_checker" namespace="">
-        <!-- topic remap -->
-        <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-        <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
-        <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
-        <remap from="~/output/max_velocity" to="/planning/scenario_planning/max_velocity_candidates"/>
-        <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit"/>
-        <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-        <remap from="~/output/no_start_reason" to="/planning/scenario_planning/status/no_start_reason"/>
-        <!-- params -->
-        <param from="$(var common_param_path)"/>
-        <param from="$(var vehicle_param_file)"/>
-        <param from="$(var nearest_search_param_path)"/>
-        <param from="$(var surround_obstacle_checker_param_path)"/>
-        <!-- composable node config -->
-        <extra_arg name="use_intra_process_comms" value="false"/>
-      </composable_node>
-    </load_composable_node>
-  </group>
 </launch>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -2,7 +2,7 @@
   <arg name="enable_all_modules_auto_mode"/>
   <arg name="is_simulation"/>
   <!-- scenario selector -->
-  <group>
+  <!-- <group>
     <include file="$(find-pkg-share autoware_scenario_selector)/launch/scenario_selector.launch.xml">
       <arg name="input_lane_driving_trajectory" value="/planning/scenario_planning/lane_driving/trajectory"/>
       <arg name="input_parking_trajectory" value="/planning/scenario_planning/parking/trajectory"/>
@@ -13,7 +13,7 @@
       <arg name="output_trajectory" value="/planning/scenario_planning/scenario_selector/trajectory"/>
       <arg name="is_parking_completed" value="/planning/scenario_planning/parking/is_completed"/>
     </include>
-  </group>
+  </group> -->
 
   <!-- velocity planning with max velocity, acceleration, jerk, stop point constraint -->
   <group>
@@ -36,7 +36,7 @@
 
           <param name="publish_debug_trajs" value="false"/>
           <remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
-          <remap from="~/output/trajectory" to="/planning/scenario_planning/velocity_smoother/trajectory"/>
+          <remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory"/>
 
           <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity"/>
           <remap from="~/input/acceleration" to="/localization/acceleration"/>
@@ -58,10 +58,10 @@
       </include>
     </group>
     <!-- parking -->
-    <group>
+    <!-- <group>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/parking.launch.xml">
         <arg name="container_type" value="component_container_mt"/>
       </include>
-    </group>
+    </group> -->
   </group>
 </launch>
diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp
@@ -203,6 +203,7 @@ rcl_interfaces::msg::SetParametersResult PathSampler::onParam(
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   result.reason = "success";
+  resetPreviousData();
   return result;
 }
 
@@ -433,7 +434,10 @@ std::vector<autoware::sampler_common::Path> PathSampler::generateCandidatesFromP
       reuse_state.heading = reused_path.yaws.back();
       reuse_state.frenet = path_spline.frenet(reuse_state.pose);
       auto paths = generateCandidatePaths(reuse_state, path_spline, reuse_length, params_);
-      for (auto & p : paths) candidates.push_back(reused_path.extend(p));
+      for (auto & p : paths) {
+        p.cost = -100.0;
+        candidates.push_back(reused_path.extend(p));
+      }
     }
   }
   return candidates;
diff --git a/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp
@@ -38,9 +38,13 @@ void calculateLengthCost(Path & path, const Constraints & constraints)
 void calculateLateralDeviationCost(
   Path & path, const Constraints & constraints, const transform::Spline2D & reference)
 {
-  const auto fp = reference.frenet(path.points.back());
-  const double lateral_deviation = std::abs(fp.d);
-  path.cost += constraints.soft.lateral_deviation_weight * lateral_deviation;
+  auto sum_lateral_deviation = 0.0;
+  for(const auto & p : path.points) {
+      const auto fp = reference.frenet(p);
+      const double lateral_deviation = std::abs(fp.d);
+    sum_lateral_deviation += lateral_deviation;
+  }
+  path.cost += constraints.soft.lateral_deviation_weight * (sum_lateral_deviation / path.points.size());
 }
 
 void calculateCost(

Original file line number	Diff line number	Diff line change
`@@ -203,6 +203,7 @@ rcl_interfaces::msg::SetParametersResult PathSampler::onParam(`
`203`	`203`	`rcl_interfaces::msg::SetParametersResult result;`
`204`	`204`	`result.successful = true;`
`205`	`205`	`result.reason = "success";`
	`206`	`+ resetPreviousData();`
`206`	`207`	`return result;`
`207`	`208`	`}`
`208`	`209`
`@@ -433,7 +434,10 @@ std::vector<autoware::sampler_common::Path> PathSampler::generateCandidatesFromP`
`433`	`434`	`reuse_state.heading = reused_path.yaws.back();`
`434`	`435`	`reuse_state.frenet = path_spline.frenet(reuse_state.pose);`
`435`	`436`	`auto paths = generateCandidatePaths(reuse_state, path_spline, reuse_length, params_);`
`436`		`- for (auto & p : paths) candidates.push_back(reused_path.extend(p));`
	`437`	`+ for (auto & p : paths) {`
	`438`	`+ p.cost = -100.0;`
	`439`	`+ candidates.push_back(reused_path.extend(p));`
	`440`	`+ }`
`437`	`441`	`}`
`438`	`442`	`}`
`439`	`443`	`return candidates;`