diff --git a/.circleci/config.yml b/.circleci/config.yml index 94409020586..32f1803fc70 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -28,7 +28,7 @@ jobs: # Pull docker image from docker hub # XTERM used for better catkin_make output docker: - - image: usdotfhwastol/carma-base:carma-system-4.1.0 + - image: usdotfhwastol/carma-base:carma-system-4.2.0 user: carma environment: TERM: xterm diff --git a/.dockerignore b/.dockerignore index 1dbd884144b..450c7dc804a 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,6 +1,49 @@ -.git* -ros/build* -ros/devel* -ros/install* -ros/log* -ros/coverage_* +# Added colcon build folders +# Kyle Rush - 10/30/2019 + +# GNU global tags +GTAGS +GRTAGS +GSYMS +GPATH + +# Python byte-compile file +__pycache__/ +*.py[co] + +# editor backup files +*~ +\#*\# + +# backup files +*.bak + +# Eclipse +.cproject +.project +.pydevproject +.settings/ + +# Visual Studio Code +**/.vscode/ + +# CLion +.idea/ +cmake-build-debug/ + +# clang-format +.clang-format + +# ROSBag files +*.bag + +# Autoware Resources +*.pcd +*.bag +*.csv + +/.metadata/ +build/ +install/ +log/ +.git/ diff --git a/Dockerfile b/Dockerfile index bdcb163312e..1d5d2758ba1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,6 +1,11 @@ -FROM usdotfhwastol/carma-base:carma-system-4.1.0 as base_image +FROM usdotfhwastol/carma-base:carma-system-4.2.0 AS base_image -FROM base_image as build +FROM base_image AS build + +ARG ROS1_PACKAGES="" +ENV ROS1_PACKAGES=${ROS1_PACKAGES} +ARG ROS2_PACKAGES="" +ENV ROS2_PACKAGES=${ROS2_PACKAGES} COPY --chown=carma . /home/carma/autoware.ai RUN /home/carma/autoware.ai/docker/checkout.bash diff --git a/common/autoware_build_flags/CMakeLists.txt b/common/autoware_build_flags/CMakeLists.txt index 79f2c3d6c6d..eab8f612fe5 100644 --- a/common/autoware_build_flags/CMakeLists.txt +++ b/common/autoware_build_flags/CMakeLists.txt @@ -1,6 +1,19 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(autoware_build_flags) -find_package(catkin REQUIRED) -catkin_package(CFG_EXTRAS autoware_build_flags-extras.cmake) +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) + +if(${ROS_VERSION} EQUAL 1) + + find_package(catkin REQUIRED) + catkin_package(CFG_EXTRAS autoware_build_flags-extras.cmake) + +else() #ROS2 + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + ament_auto_package() + +endif() diff --git a/common/autoware_build_flags/package.xml b/common/autoware_build_flags/package.xml index da95670d10a..90fd9743b02 100644 --- a/common/autoware_build_flags/package.xml +++ b/common/autoware_build_flags/package.xml @@ -1,7 +1,7 @@ - + autoware_build_flags - 1.12.0 + 1.2.0 Common build flags for Autoware. Esteve Fernandez @@ -9,7 +9,8 @@ Apache 2 - catkin + catkin + ament_cmake_auto diff --git a/common/lanelet2_extension/CMakeLists.txt b/common/lanelet2_extension/CMakeLists.txt index c56e6d6795b..fb96f5b823e 100644 --- a/common/lanelet2_extension/CMakeLists.txt +++ b/common/lanelet2_extension/CMakeLists.txt @@ -62,6 +62,7 @@ set(cpp_files lib/SignalizedIntersection.cpp lib/RegionAccessRule.cpp lib/DirectionOfTravel.cpp + lib/TimeConversion.cpp ) diff --git a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h index 5fc1b102960..888e30f0b95 100644 --- a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h +++ b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h @@ -19,6 +19,7 @@ #include #include +#include #include namespace lanelet @@ -58,42 +59,6 @@ struct CarmaTrafficSignalRoleNameString static constexpr char ControlEnd[] = "control_end"; }; -// Namespace for time representations used with this regulatory element -namespace time { - - /** - * \brief Converts a boost time duration into the number of posix seconds it represents - * - * \param duration The duration to convert. - * \return The number of posix seconds with microsecond resolution - */ - double toSec(const boost::posix_time::time_duration& duration); - - /** - * \brief Converts a boost time duration into the number of posix seconds since 1970 - * - * \param duration The duration to convert. - * \return The number of posix seconds since 1970 with microsecond resolution - */ - double toSec(const boost::posix_time::ptime& time); - - /** - * \brief Returns a boost posix time object which matches the input posix seconds since 1970 with microsecond accuracy. - * - * \param sec The number of posix seconds since 1970. Fractional seconds are supported. - * \return Initialized posix time object matching the input - */ - boost::posix_time::ptime timeFromSec(double sec); - - /** - * \brief Returns a boost posix time object which matches the input posix seconds duration with microsecond accuracy. - * - * \param sec The number of posix seconds the duration should reflect. Fractional seconds are supported. - * \return Initialized posix time duration object matching the input - */ - boost::posix_time::time_duration durationFromSec(double sec); -} - /** * \brief Stream operator for CarmaTrafficSignalState enum. */ diff --git a/common/lanelet2_extension/include/lanelet2_extension/time/TimeConversion.h b/common/lanelet2_extension/include/lanelet2_extension/time/TimeConversion.h new file mode 100644 index 00000000000..ca7db816e9b --- /dev/null +++ b/common/lanelet2_extension/include/lanelet2_extension/time/TimeConversion.h @@ -0,0 +1,34 @@ +#pragma once +/* + * Copyright (C) 2022 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include + +namespace lanelet +{ + +namespace time { + +double toSec(const boost::posix_time::time_duration& duration); + +double toSec(const boost::posix_time::ptime& time); + +boost::posix_time::ptime timeFromSec(double sec); + +boost::posix_time::time_duration durationFromSec(double sec); + +} // namespace time + +} // namespace lanelet \ No newline at end of file diff --git a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp index 0679f484820..bd6bcc0ffb8 100755 --- a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp +++ b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp @@ -129,8 +129,8 @@ boost::optional> Ca boost::posix_time::time_duration accumulated_offset_duration; double offset_duration_dir = recorded_time_stamps.front().first > time_stamp ? -1.0 : 1.0; // -1 if past, +1 if time_stamp is in future - int num_of_cycles = std::abs(toSec(recorded_time_stamps.front().first - time_stamp) / toSec(fixed_cycle_duration)); - accumulated_offset_duration = durationFromSec( num_of_cycles * toSec(fixed_cycle_duration)); + int num_of_cycles = std::abs(lanelet::time::toSec(recorded_time_stamps.front().first - time_stamp) / lanelet::time::toSec(fixed_cycle_duration)); + accumulated_offset_duration = durationFromSec( num_of_cycles * lanelet::time::toSec(fixed_cycle_duration)); if (offset_duration_dir < 0) { @@ -142,8 +142,8 @@ boost::optional> Ca // iterate through states in the cycle to get the signal for (size_t i = 0; i < recorded_time_stamps.size(); i++) { - double end_time = toSec(recorded_time_stamps[i].first) + offset_duration_dir * toSec(accumulated_offset_duration); - if (end_time >= toSec(time_stamp)) + double end_time = lanelet::time::toSec(recorded_time_stamps[i].first) + offset_duration_dir * lanelet::time::toSec(accumulated_offset_duration); + if (end_time >= lanelet::time::toSec(time_stamp)) { return std::pair(timeFromSec(end_time), recorded_time_stamps[i].second); } @@ -198,32 +198,6 @@ void CarmaTrafficSignal::setStates(std::vector(sec * 1000000L)); -} - -boost::posix_time::time_duration durationFromSec(double sec) { - return boost::posix_time::microseconds(static_cast(sec * 1000000L)); -} - -} // namespace time - - - namespace { // this object actually does the registration work for us diff --git a/common/lanelet2_extension/lib/TimeConversion.cpp b/common/lanelet2_extension/lib/TimeConversion.cpp new file mode 100644 index 00000000000..e60979c2c51 --- /dev/null +++ b/common/lanelet2_extension/lib/TimeConversion.cpp @@ -0,0 +1,46 @@ + +/* + * Copyright (C) 2022 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include +#include + +namespace lanelet +{ + +namespace time { + +double toSec(const boost::posix_time::time_duration& duration) { + if (duration.is_special()) { + throw std::invalid_argument("Cannot convert special duration to seconds"); + } + return duration.total_microseconds() / 1000000.0; +} + +double toSec(const boost::posix_time::ptime& time) { + return toSec(time - boost::posix_time::from_time_t(0)); +} + +boost::posix_time::ptime timeFromSec(double sec) { + return boost::posix_time::from_time_t(0) + boost::posix_time::microseconds(static_cast(sec * 1000000L)); +} + +boost::posix_time::time_duration durationFromSec(double sec) { + return boost::posix_time::microseconds(static_cast(sec * 1000000L)); +} + +} // namespace time + +} // namespace lanelet \ No newline at end of file diff --git a/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h b/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h index 2e4caa32fef..c2f4965ef38 100644 --- a/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h +++ b/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h @@ -80,7 +80,7 @@ class PurePursuitNode // control loop update rate double update_rate_; -const int DEFAULT_VELOCITY_SOURCE_ = 0; + const int DEFAULT_VELOCITY_SOURCE_ = 0; const double DEFAULT_CONST_VELOCITY_ = 5.0; // variables @@ -98,6 +98,12 @@ const int DEFAULT_VELOCITY_SOURCE_ = 0; double minimum_lookahead_distance_; std::string output_interface_; + // parameters for controller shutdown + long consecutive_input_counter_ = 0; + int ignore_initial_inputs_ = 0; + long prev_input_time_ = 0; + int shutdown_timeout_ = 200; + // callbacks void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg); diff --git a/core_planning/pure_pursuit/launch/pure_pursuit.launch b/core_planning/pure_pursuit/launch/pure_pursuit.launch index 166f602e3ff..493dba79e70 100644 --- a/core_planning/pure_pursuit/launch/pure_pursuit.launch +++ b/core_planning/pure_pursuit/launch/pure_pursuit.launch @@ -8,6 +8,8 @@ + + @@ -23,5 +25,7 @@ + + diff --git a/core_planning/pure_pursuit/src/pure_pursuit_core.cpp b/core_planning/pure_pursuit/src/pure_pursuit_core.cpp index 6a765e1a96f..b2bf3a6bc78 100644 --- a/core_planning/pure_pursuit/src/pure_pursuit_core.cpp +++ b/core_planning/pure_pursuit/src/pure_pursuit_core.cpp @@ -60,6 +60,8 @@ void PurePursuitNode::initForROS() private_nh_.param("out_twist_name", out_twist, std::string("twist_raw")); private_nh_.param("out_ctrl_cmd_name", out_ctrl_cmd, std::string("ctrl_raw")); private_nh_.param("output_interface", output_interface_, std::string("ctrl_cmd")); + private_nh_.param("ignore_initial_inputs", ignore_initial_inputs_, 0); + private_nh_.param("shutdown_timeout", shutdown_timeout_, 200); // Output type, use old parameter name only if it is set @@ -119,6 +121,25 @@ void PurePursuitNode::run() [this](const auto&) { + long current_time = ros::Time::now().toNSec() / 1000000; + ROS_DEBUG_STREAM("current_time = " << current_time << ", prev_input_time_ = " << prev_input_time_ << ", input counter = " << consecutive_input_counter_); + // If it has been a long time since input data has arrived then reset the input counter and return so control signals arent published anymore + if (current_time - prev_input_time_ > shutdown_timeout_) + { + ROS_DEBUG_STREAM("returning due to timeout."); + consecutive_input_counter_ = 0; + is_waypoint_set_ = false; + return; + } + + // If there have not been enough consecutive timely inputs then return (waiting for + // previous control plugin to time out and stop publishing, since it uses same output topic) + if (consecutive_input_counter_ <= ignore_initial_inputs_) + { + ROS_DEBUG_STREAM("returning due to first data input"); + return; + } + if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_) // One time check on desired input data { ROS_DEBUG("Necessary topics are not subscribed yet ... "); @@ -303,6 +324,13 @@ void PurePursuitNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStam void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg) { + + prev_input_time_ = ros::Time::now().toNSec() / 1000000; + ++consecutive_input_counter_; + ROS_DEBUG_STREAM("New trajectory plan #" << consecutive_input_counter_ << " at time " << prev_input_time_); + // TODO prev_input_time_ replaced by msg timestamp, if the value is valid during tests + ROS_DEBUG_STREAM("lane msg header time = " << msg->header.stamp.toNSec() / 1000000); + if (add_virtual_end_waypoints_) { diff --git a/docker/build-image.sh b/docker/build-image.sh index 072efce9bb8..a4f5e4aebad 100755 --- a/docker/build-image.sh +++ b/docker/build-image.sh @@ -13,10 +13,11 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations under # the License. + USERNAME=usdotfhwastol cd "$(dirname "$0")" -IMAGE=$('./get-image-name.sh') +IMAGE=$(basename `git rev-parse --show-toplevel`) echo "" echo "##### $IMAGE Docker Image Build Script #####" @@ -43,9 +44,59 @@ while [[ $# -gt 0 ]]; do COMPONENT_VERSION_STRING=develop shift ;; + --ros-1-packages|--ros1) + ROS1_PACKAGES="" + shift + ;; + --ros-2-packages|--ros2) + ROS2_PACKAGES="" + shift + ;; + *) + # Var test based on Stack Overflow question: https://stackoverflow.com/questions/5406858/difference-between-unset-and-empty-variables-in-bash + # Asker: green69 + # Answerer: geekosaur + if [ "${ROS2_PACKAGES+set}" = "set" ]; then + ROS2_PACKAGES="$ROS2_PACKAGES $arg" + elif [ "${ROS1_PACKAGES+set}" = "set" ]; then + ROS1_PACKAGES="$ROS1_PACKAGES $arg" + else + echo "Unknown argument $arg..." + exit -1 + fi + shift + ;; esac done +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Performing incremental build of image to rebuild packages: $ROS1_PACKAGES $ROS2_PACKAGES..." + + echo "Updating Dockerfile references to use most recent image as base image" + # Trim of docker image LS command sourced from + # https://stackoverflow.com/questions/50625619/why-doesnt-the-cut-command-work-for-a-docker-image-ls-command + # Question Asker: Chris F + # Question Answerer: Arount + MOST_RECENT_IMAGE_DATA=$(docker image ls | grep $IMAGE | tr -s ' ') + + if [[ -z "$MOST_RECENT_IMAGE_DATA" ]]; then + echo No prior image exists to use as base, an initial image must be built first before attempting incremental build. + exit -1 + fi + + MOST_RECENT_IMAGE_HASH=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 3) + MOST_RECENT_IMAGE_ORG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 1 | cut -d "/" -f 1) + MOST_RECENT_IMAGE_TAG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 2) + MOST_RECENT_IMAGE_DATE=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 4,5,6) + + echo Using $MOST_RECENT_IMAGE_TAG $MOST_RECENT_IMAGE_HASH $MOST_RECENT_IMAGE_DATE as base for partial build... + + sed -i "s|^FROM[[:space:]]*[^[:space:]]*|FROM $MOST_RECENT_IMAGE_HASH|I" ../Dockerfile + + COMPONENT_VERSION_STRING="SNAPSHOT" + USERNAME="local" +fi + if [[ -z "$COMPONENT_VERSION_STRING" ]]; then COMPONENT_VERSION_STRING=$("./get-component-version.sh") fi @@ -55,12 +106,22 @@ echo "Final image name: $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING" cd .. if [[ $COMPONENT_VERSION_STRING = "develop" ]]; then - sed "s|usdotfhwastoldev/|$USERNAME/|g; s|usdotfhwastolcandidate/|$USERNAME/|g; s|usdotfhwastol/|$USERNAME/|g; s|:*[0-9].*[0-9].*[0-9]|:$COMPONENT_VERSION_STRING|g; s|checkout.bash|checkout.bash -d|g" \ + sed "s|usdotfhwastoldev/|$USERNAME/|g; s|usdotfhwastolcandidate/|$USERNAME/|g; s|usdotfhwastol/|$USERNAME/|g; s|:[0-9]*\.[0-9]*\.[0-9]*|:$COMPONENT_VERSION_STRING|g; s|checkout.bash|checkout.bash -d|g" \ Dockerfile | docker build -f - --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ --build-arg VERSION="$COMPONENT_VERSION_STRING" \ --build-arg VCS_REF=`git rev-parse --short HEAD` \ --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . +elif [[ $COMPONENT_VERSION_STRING = "SNAPSHOT" ]]; then + docker build --network=host --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ + --build-arg ROS1_PACKAGES="$ROS1_PACKAGES" \ + --build-arg ROS2_PACKAGES="$ROS2_PACKAGES" \ + --build-arg VERSION="$COMPONENT_VERSION_STRING" \ + --build-arg VCS_REF=`git rev-parse --short HEAD` \ + --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . else + #The addition of --network=host was a fix for a DNS resolution error that occured + #when running the platform inside an Ubuntu 20.04 virtual machine. The error and possible soliutions are + # discussed here: https://github.com/moby/moby/issues/41003 docker build --network=host --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ --build-arg VERSION="$COMPONENT_VERSION_STRING" \ --build-arg VCS_REF=`git rev-parse --short HEAD` \ @@ -89,4 +150,4 @@ if [ "$PUSH" = true ]; then fi echo "" -echo "##### $IMAGE Docker Image Build Done! #####" \ No newline at end of file +echo "##### $IMAGE Docker Image Build Done! #####" diff --git a/docker/checkout.bash b/docker/checkout.bash index adda8629ff1..d29693f82aa 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -39,9 +39,9 @@ if [[ "$BRANCH" = "develop" ]]; then git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch $BRANCH git clone --depth=1 https://github.com/usdot-fhwa-stol/autoware.auto.git --branch $BRANCH else - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.1.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.1.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/autoware.auto.git --branch carma-system-4.1.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.2.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.2.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/autoware.auto.git --branch carma-system-4.2.0 fi # Required to build pacmod_msgs diff --git a/docker/install.sh b/docker/install.sh index 54021ff002a..eef77314c6a 100755 --- a/docker/install.sh +++ b/docker/install.sh @@ -19,7 +19,13 @@ # ROS 1 Build ### # Source environment variables -source /home/carma/.base-image/init-env.sh +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Sourcing previous build for incremental build start point..." + source /opt/autoware.ai/ros/install/setup.bash +else + echo "Sourcing base image for full build..." + source /home/carma/.base-image/init-env.sh +fi # Enter source directory cd /home/carma/autoware.ai @@ -29,7 +35,18 @@ echo "ROS 1 Build with CUDA" sudo mkdir /opt/autoware.ai # Create install directory sudo chown carma /opt/autoware.ai # Set owner to expose permissions for build sudo chgrp carma /opt/autoware.ai # Set group to expose permissions for build -AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --build-base build_ros1 --install-base /opt/autoware.ai/ros/install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs -DCMAKE_CXX_FLAGS=-Wall -DCMAKE_C_FLAGS=-Wall + +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + if [[ ! -z "$ROS1_PACKAGES" ]]; then + echo "Incrementally building ROS1 packages: $ROS1_PACKAGES" + AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --build-base build_ros1 --install-base /opt/autoware.ai/ros/install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs -DCMAKE_CXX_FLAGS=-Wall -DCMAKE_C_FLAGS=-Wall --packages-above $ROS1_PACKAGES --allow-overriding $ROS1_PACKAGES + else + echo "Build type is incremental but no ROS1 packages specified, skipping ROS1 build..." + fi +else + echo "Building all ROS1 Autoware.AI Components" + AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --build-base build_ros1 --install-base /opt/autoware.ai/ros/install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs -DCMAKE_CXX_FLAGS=-Wall -DCMAKE_C_FLAGS=-Wall +fi # Get the exit code from the ROS1 build so we can skip the ROS2 build if the ROS1 build failed status=$? @@ -38,13 +55,30 @@ if [[ $status -ne 0 ]]; then echo "Autoware.ai build failed." exit $status fi +echo "Build of ROS1 Autoware.AI Components Complete" ### # ROS 2 Build ### source /opt/ros/foxy/setup.bash -source /home/carma/catkin/setup.bash +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Sourcing previous build for incremental build start point..." + source /opt/autoware.ai/ros/install_ros2/setup.bash +else + echo "Sourcing base image for full build..." + source /home/carma/catkin/setup.bash +fi echo "ROS 2 Build" -colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + if [[ ! -z "$ROS2_PACKAGES" ]]; then + echo "Incrementally building ROS2 packages: $ROS1_PACKAGES" + colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS2_PACKAGES --allow-overriding $ROS2_PACKAGES + else + echo "Build type is incremental but no ROS2 packages specified, skipping ROS2 build..." + fi +else + echo "Building all ROS2 Autoware.AI Components" + colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release +fi