From 52acf8152787b3acd87cadde470467d051ab4979 Mon Sep 17 00:00:00 2001
From: AnonymDavid <jodavid512@gmail.com>
Date: Thu, 7 Nov 2024 15:38:06 +0100
Subject: [PATCH] Created core launch; buld_all uses build_core

---
 launch/crp_launcher/launch/core.launch.py | 118 ++++++++++++++++++++++
 scripts/build_all.sh                      |  35 ++++++-
 scripts/build_core.sh                     |   6 +-
 3 files changed, 154 insertions(+), 5 deletions(-)
 create mode 100644 launch/crp_launcher/launch/core.launch.py

diff --git a/launch/crp_launcher/launch/core.launch.py b/launch/crp_launcher/launch/core.launch.py
new file mode 100644
index 0000000..efa4491
--- /dev/null
+++ b/launch/crp_launcher/launch/core.launch.py
@@ -0,0 +1,118 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import LaunchConfigurationEquals
+from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+from os.path import join
+
+
+def generate_launch_description():
+    
+    # ARGS
+
+    # sensor abstraction
+    vehicle_tire_angle_topic_arg = DeclareLaunchArgument(
+        'vehicle_tire_angle_topic',
+        default_value='/sensing/vehicle/tire_angle',
+        description='Length of the scenario in meters')
+    local_path_length_arg = DeclareLaunchArgument(
+        'local_path_length',
+        default_value='70.0',
+        description='Length of the scenario in meters')
+    
+    # NODES
+
+    sensor_abstraction = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('prcp_sensor_abstraction'),
+                'launch',
+                'sensor_abstraction.launch.py')
+        )
+    )
+
+    environmental_fusion = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('prcp_situation_analysis'),
+                'launch',
+                'environmental_fusion.launch.py')
+        )
+    )
+
+    behavior_planning = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_behavior_planning'),
+                'launch',
+                'behavior_planning.launch.py'
+            )
+        )
+    )
+
+    motion_planning = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_motion_planning'),
+                'launch',
+                'motion_planning.launch.py'
+            )
+        )
+    )
+
+    planner_lat_lane_follow_ldm = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_lat_lane_follow_ldm'),
+                'launch',
+                'plan_lat_lane_follow_ldm.launch.py'
+            )
+        )
+    )
+
+    vehicle_control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control'),
+                'launch',
+                'ctrl_vehicle_control.launch.py')
+        )
+    )
+
+    vehicle_control_lat = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control_lat_compensatory'),
+                'launch',
+                'ctrl_vehicle_control_lat_compensatory.launch.py')
+        )
+    )
+
+    vehicle_control_long = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control_long'),
+                'launch',
+                'ctrl_vehicle_control_long.launch.py')
+        )
+    )
+
+    return LaunchDescription([
+        # args
+        vehicle_tire_angle_topic_arg,
+        vehicle_tire_angle_topic_arg,
+        local_path_length_arg,
+
+        # nodes
+        sensor_abstraction,
+        environmental_fusion,
+        behavior_planning,
+        motion_planning,
+        vehicle_control,
+        vehicle_control_lat,
+        vehicle_control_long,
+
+        planner_lat_lane_follow_ldm
+    ])
diff --git a/scripts/build_all.sh b/scripts/build_all.sh
index 493ae2c..cded75e 100755
--- a/scripts/build_all.sh
+++ b/scripts/build_all.sh
@@ -3,7 +3,36 @@ cd $script_dir
 while [ ! -e "src/" ]; do
   cd ../
 done
-echo $(pwd)
+echo "Build location: "$(pwd)
 
-rosdep install --from-paths src --ignore-src -r -y
-colcon build
+# build core
+trap 'exit' INT # trap ctrl-c
+$script_dir/build_core.sh
+trap - INT
+
+# build lexus packages
+packages=(
+  duro_gps_driver
+  duro_gps_launcher
+  kvaser_interface
+  lanelet_handler
+  lexus_bringup
+  mcap_rec
+  mpc_camera_driver
+  novatel_gps_msgs
+  novatel_gps_driver
+  novatel_gps_launcher
+  pacmod_extender
+  pacmod_interface
+)
+
+packages_string=""
+for package in "${packages[@]}"; do
+  packages_string+="$package "
+done
+
+packages_paths=$(colcon list --packages-up-to $packages_string -p)
+
+rosdep install --from-paths $packages_paths --ignore-src -r -y
+
+colcon build --packages-select $packages_string
\ No newline at end of file
diff --git a/scripts/build_core.sh b/scripts/build_core.sh
index ac3bf53..03685d5 100755
--- a/scripts/build_core.sh
+++ b/scripts/build_core.sh
@@ -3,9 +3,9 @@ cd $script_dir
 while [ ! -e "src/" ]; do
   cd ../
 done
-echo $(pwd)
+echo "Build location: "$(pwd)
 
-rosdep install --from-paths src --ignore-src -r -y
+rosdep install --from-paths $script_dir/../crp_APL $script_dir/../crp_CIL --ignore-src -r -y
 colcon build --packages-select \
 tier4_planning_msgs \
 autoware_common_msgs \
@@ -26,4 +26,6 @@ plan_lon_intelligent_speed_adjust \
 plan_motion_planning \
 crp_launcher \
 ctrl_vehicle_control \
+ctrl_vehicle_control_lat_compensatory \
+ctrl_vehicle_control_long \
 test_node