From 22f4d915bdb1f04cdc8826f5ed09223d53c035f4 Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Wed, 6 Nov 2024 15:40:52 +0100 Subject: [PATCH 1/4] Added rosdep to scripts, removed deps from launcher --- launch/crp_launcher/package.xml | 9 --------- scripts/build_all.sh | 3 ++- scripts/build_core.sh | 10 +++++++++- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/launch/crp_launcher/package.xml b/launch/crp_launcher/package.xml index 02f9695..eb7cca5 100644 --- a/launch/crp_launcher/package.xml +++ b/launch/crp_launcher/package.xml @@ -9,22 +9,13 @@ ament_cmake - - novatel_gps_launcher tf2_ros - kvaser_interface - lanelet_handler prcp_sensor_abstraction prcp_situation_analysis plan_behavior_planning plan_motion_planning plan_lat_lane_follow_ldm ctrl_vehicle_control - - nissan_bringup - - lexus_bringup - pacmod3 ament_cmake diff --git a/scripts/build_all.sh b/scripts/build_all.sh index b5d7abe..39b764b 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -5,4 +5,5 @@ while [ ! -e "src/" ]; do done echo $(pwd) -colcon build --symlink-install \ No newline at end of file +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install diff --git a/scripts/build_core.sh b/scripts/build_core.sh index a227490..100d7db 100755 --- a/scripts/build_core.sh +++ b/scripts/build_core.sh @@ -5,7 +5,15 @@ while [ ! -e "src/" ]; do done echo $(pwd) +rosdep install --from-paths src --ignore-src -r -y colcon build --symlink-install --packages-select \ +tier4_planning_msgs \ +autoware_common_msgs \ +autoware_planning_msgs \ +autoware_perception_msgs \ +autoware_control_msgs \ +autoware_localization_msgs \ +autoware_map_msgs \ crp_msgs \ prcp_sensor_abstraction \ prcp_situation_analysis \ @@ -18,4 +26,4 @@ plan_lon_intelligent_speed_adjust \ plan_motion_planning \ crp_launcher \ ctrl_vehicle_control \ -test_node \ No newline at end of file +test_node From ef17a5f52fafaad7e322a9f444c4c6d9f17a36aa Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Wed, 6 Nov 2024 15:51:19 +0100 Subject: [PATCH 2/4] Removed symlink-install from scripts --- scripts/build_all.sh | 2 +- scripts/build_core.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/build_all.sh b/scripts/build_all.sh index 39b764b..493ae2c 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -6,4 +6,4 @@ done echo $(pwd) rosdep install --from-paths src --ignore-src -r -y -colcon build --symlink-install +colcon build diff --git a/scripts/build_core.sh b/scripts/build_core.sh index 100d7db..ac3bf53 100755 --- a/scripts/build_core.sh +++ b/scripts/build_core.sh @@ -6,7 +6,7 @@ done echo $(pwd) rosdep install --from-paths src --ignore-src -r -y -colcon build --symlink-install --packages-select \ +colcon build --packages-select \ tier4_planning_msgs \ autoware_common_msgs \ autoware_planning_msgs \ From 52acf8152787b3acd87cadde470467d051ab4979 Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 15:38:06 +0100 Subject: [PATCH 3/4] 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 From f31fb80b58295b5f0510c45137199f96053c1f1e Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 15:43:48 +0100 Subject: [PATCH 4/4] Lexus laucher uses core launcher --- launch/crp_launcher/launch/lexus.launch.py | 99 ++++------------------ 1 file changed, 15 insertions(+), 84 deletions(-) diff --git a/launch/crp_launcher/launch/lexus.launch.py b/launch/crp_launcher/launch/lexus.launch.py index 5b3c6c7..ef2e876 100644 --- a/launch/crp_launcher/launch/lexus.launch.py +++ b/launch/crp_launcher/launch/lexus.launch.py @@ -77,6 +77,18 @@ def generate_launch_description(): default_value='70.0', description='Length of the scenario in meters') + + # CORE + + crp_core = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('crp_launcher'), + 'launch', + 'core.launch.py') + ) + ) + # NODES novatel_gps = IncludeLaunchDescription( @@ -198,63 +210,6 @@ def generate_launch_description(): ) ) - 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') - ) - ) - lexus_speed_control = IncludeLaunchDescription( PythonLaunchDescriptionSource( join( @@ -264,24 +219,6 @@ def generate_launch_description(): ) ) - 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 select_gps_arg, @@ -300,6 +237,9 @@ def generate_launch_description(): vehicle_tire_angle_topic_arg, local_path_length_arg, + # core + crp_core, + # vehicle nodes novatel_gps, duro_gps, @@ -316,13 +256,4 @@ def generate_launch_description(): # nodes lanelet_file_loader, - sensor_abstraction, - environmental_fusion, - behavior_planning, - motion_planning, - vehicle_control, - vehicle_control_lat, - vehicle_control_long, - - planner_lat_lane_follow_ldm ])