From 7176938cdb7ed680910e84e308c2ff170d17e9f4 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Mon, 21 Oct 2024 16:01:22 +0900
Subject: [PATCH] follow old transition

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../behavior_planning.launch.xml              | 73 +++++++++----------
 .../src/interface.cpp                         |  2 +-
 2 files changed, 37 insertions(+), 38 deletions(-)

diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 53869ec619154..17d9a88b85d8a 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -180,44 +180,43 @@
   />
   <let name="behavior_velocity_planner_launch_modules" value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
 
-  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
-    <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
-      <!-- topic remap -->
-      <remap from="~/input/route" to="$(var input_route_topic_name)"/>
-      <remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>
-      <remap from="~/input/perception" to="/perception/object_recognition/objects"/>
-      <remap from="~/input/occupancy_grid_map" to="/perception/occupancy_grid_map/map"/>
-      <remap from="~/input/costmap" to="/planning/scenario_planning/parking/costmap_generator/occupancy_grid"/>
-      <remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>
-      <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-      <remap from="~/input/accel" to="/localization/acceleration"/>
-      <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
-      <remap from="~/output/path" to="path_with_lane_id"/>
-      <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
-      <remap from="~/output/hazard_lights_cmd" to="/planning/behavior_path_planner/hazard_lights_cmd"/>
-      <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
-      <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-      <!-- params -->
-      <param from="$(var common_param_path)"/>
-      <param from="$(var vehicle_param_file)"/>
-      <param from="$(var nearest_search_param_path)"/>
-      <param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
-      <param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
-      <param name="is_simulation" value="$(var is_simulation)"/>
-      <param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
-      <param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
-      <param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
-      <param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
-      <param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
-      <param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
-      <param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
-      <param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
-      <param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
-      <param from="$(var behavior_path_planner_common_param_path)"/>
-      <!-- composable node config -->
-      <extra_arg name="use_intra_process_comms" value="false"/>
-    </composable_node>
+  <node pkg="behavior_path_planner" exec="behavior_path_planner_node" name="behavior_path_planner" namespace="" launch-prefix="xterm -l -lf /home/zulfaqar/develop/aw.txt -e gdb -ex run --args">
+    <!-- topic remap -->
+    <remap from="~/input/route" to="$(var input_route_topic_name)"/>
+    <remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>
+    <remap from="~/input/perception" to="/perception/object_recognition/objects"/>
+    <remap from="~/input/occupancy_grid_map" to="/perception/occupancy_grid_map/map"/>
+    <remap from="~/input/costmap" to="/planning/scenario_planning/parking/costmap_generator/occupancy_grid"/>
+    <remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>
+    <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+    <remap from="~/input/accel" to="/localization/acceleration"/>
+    <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
+    <remap from="~/output/path" to="path_with_lane_id"/>
+    <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
+    <remap from="~/output/hazard_lights_cmd" to="/planning/behavior_path_planner/hazard_lights_cmd"/>
+    <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
+    <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+    <!-- params -->
+    <param from="$(var common_param_path)"/>
+    <param from="$(var vehicle_param_file)"/>
+    <param from="$(var nearest_search_param_path)"/>
+    <param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
+    <param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+    <param name="is_simulation" value="$(var is_simulation)"/>
+    <param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
+    <param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
+    <param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
+    <param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
+    <param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
+    <param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
+    <param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
+    <param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
+    <param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
+    <param from="$(var behavior_path_planner_common_param_path)"/>
+    <!-- composable node config -->
+  </node>
 
+  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="behavior_velocity_planner" plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
       <!-- topic remap -->
       <remap from="~/input/path_with_lane_id" to="path_with_lane_id"/>
diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
index 591b96e79a5bb..645f7ef6d8e8a 100644
--- a/planning/behavior_path_lane_change_module/src/interface.cpp
+++ b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -146,7 +146,7 @@ ModuleStatus LaneChangeInterface::updateState()
   if (!module_type_->isAbleToReturnCurrentLane()) {
     log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change.");
     change_state_if_stop_required();
-    return within_turn_direction_lane ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
+    return within_turn_direction_lane ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING;
   }
 
   const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane;