Merge pull request #228 from usdot-fhwa-stol/release/cabin

Release/cabin

.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

.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/

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

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()

common/autoware_build_flags/package.xml

@@ -1,15 +1,16 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>autoware_build_flags</name>
-  <version>1.12.0</version>
+  <version>1.2.0</version>
   <description>Common build flags for Autoware.</description>
 
   <author email="[email protected]">Esteve Fernandez</author>
   <maintainer email="[email protected]">Esteve Fernandez</maintainer>
 
   <license>Apache 2</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_auto</buildtool_depend>
 
   <export>
     <architecture_independent/>

common/lanelet2_extension/CMakeLists.txt

@@ -62,6 +62,7 @@ set(cpp_files
   lib/SignalizedIntersection.cpp
   lib/RegionAccessRule.cpp
   lib/DirectionOfTravel.cpp
+  lib/TimeConversion.cpp
 )
 
 

common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h

@@ -19,6 +19,7 @@
 
 #include <lanelet2_core/primitives/RegulatoryElement.h>
 #include <boost/date_time/posix_time/posix_time.hpp>
+#include <lanelet2_extension/time/TimeConversion.h>
 #include <unordered_map>
 
 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.
  */

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 <boost/date_time/posix_time/posix_time.hpp>
+
+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

common/lanelet2_extension/lib/CarmaTrafficSignal.cpp

@@ -129,8 +129,8 @@ boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> 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<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> 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<boost::posix_time::ptime, CarmaTrafficSignalState>(timeFromSec(end_time), recorded_time_stamps[i].second);
     }
@@ -198,32 +198,6 @@ void CarmaTrafficSignal::setStates(std::vector<std::pair<boost::posix_time::ptim
   revision_ = revision;
 }
 
-
-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<long>(sec * 1000000L));
-}
-
-boost::posix_time::time_duration durationFromSec(double sec) {
-  return boost::posix_time::microseconds(static_cast<long>(sec * 1000000L));
-}
-
-} // namespace time
-
-
-
 namespace
 {
 // this object actually does the registration work for us

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 <boost/date_time/posix_time/posix_time.hpp>
+#include <lanelet2_extension/time/TimeConversion.h>
+
+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<long>(sec * 1000000L));
+}
+
+boost::posix_time::time_duration durationFromSec(double sec) {
+  return boost::posix_time::microseconds(static_cast<long>(sec * 1000000L));
+}
+
+} // namespace time
+
+}  // namespace lanelet

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);

core_planning/pure_pursuit/launch/pure_pursuit.launch

@@ -8,6 +8,8 @@
   <arg name="lookahead_ratio" default="2.0"/>
   <arg name="minimum_lookahead_distance" default="6.0"/>
   <arg name="vehicle_wheel_base" default="2.79"/>
+  <arg name="shutdown_timeout" default="200"/>
+  <arg name="ignore_initial_inputs" default="0"/>
 
   <!-- 0 = waypoints, 1 = provided constant velocity -->
   <arg name="velocity_source" default="0"/>
@@ -23,5 +25,7 @@
     <param name="minimum_lookahead_distance" value="$(arg minimum_lookahead_distance)"/>
     <param name="velocity_source" value="$(arg velocity_source)"/>
     <param name="vehicle_wheel_base" value="$(arg vehicle_wheel_base)"/>
+    <param name="shutdown_timeout" value="$(arg shutdown_timeout)"/>
+    <param name="ignore_initial_inputs" value="$(arg ignore_initial_inputs)"/>
   </node>
 </launch>