Merge pull request #199 from usdot-fhwa-stol/release/fiesta

Release/fiesta

.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-3.10.0
+      - image: usdotfhwastol/carma-base:carma-system-3.11.0
         user: carma
         environment:
           TERM: xterm

Dockerfile

@@ -1,4 +1,4 @@
-FROM usdotfhwastol/carma-base:carma-system-3.10.0 as base_image
+FROM usdotfhwastol/carma-base:carma-system-3.11.0 as base_image
 
 FROM base_image as build
 

common/lanelet2_extension/CMakeLists.txt

@@ -85,11 +85,12 @@ add_library( lanelet2_extension_lib
   lib/local_frame_projector.cpp
   lib/mgrs_projector.cpp
   lib/CarmaUSTrafficRules.cpp
-  lib/CarmaTrafficLight.cpp
+  lib/CarmaTrafficSignal.cpp
   lib/PassingControlLine.cpp
   lib/StopRule.cpp
   lib/DigitalSpeedLimit.cpp
   lib/DigitalMinimumGap.cpp
+  lib/SignalizedIntersection.cpp
   lib/RegionAccessRule.cpp
   lib/DirectionOfTravel.cpp
 )
@@ -126,12 +127,13 @@ if(CATKIN_ENABLE_TESTING)
   test/src/PassingControlLineTest.cpp
   test/src/StopRuleTest.cpp
   test/src/DirectionOfTravelTest.cpp
+  test/src/SignalizedIntersectionTest.cpp
   test/src/DigitalSpeedLimitTest.cpp
   test/src/DigitalMinimumGapTest.cpp
   test/src/CarmaUSTrafficRulesTest.cpp
   test/src/MapLoadingTest.cpp
   test/src/test_local_projector.cpp
-  test/src/CarmaTrafficTest.cpp
+  test/src/CarmaTrafficSignalTest.cpp
   WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests
  )
 

common/lanelet2_extension/docs/RegulatoryElements.md

@@ -309,3 +309,155 @@ To support multiple types of participants a new attribute should be added for ea
 </relation>
 
 ```
+## Carma Traffic Signal
+
+Represents a virtual traffic signal with known fixed signal phases. It is virtual because it corresponds to single signal group ID provided by a SAE J2735 SPaT message.
+Normally the traffic signal timing information is provided by SAE J2735 SPaT messages although alternative data sources can be supported.
+
+A CarmaTrafficSignal is created from a stop_line (LineString3d) and a list of control_start and control_end lanelets. stop_line is where the vehicle should stop and wait before crossing and control_end represent which direction the vehicle can go based on this traffic signal from lanelets in control_start.
+
+### Parameters
+
+| **Role** | **Possible Type** | **description**                |
+|-------------|--------------|----------------------------------|
+| **ref_line**    | **LineString3d**    | The linestring which define the geometry of this stop line. |
+| **control_start**    **Lanelet**    | A lanelet representing the start of this traffic signal's control |
+| **control_end**    | **Lanelet**    | A lanelet representing the end of this traffic signal's control |
+
+### Custom Attributes
+
+| **Key** | **Value Type** | **description**                |
+|-------------|--------------|----------------------------------|
+| **subtype** | **carma_traffic_signal**    | Subtype name |
+
+### Note on the creation/usage of this object
+Currently when the object is created, it doesn't have any default values for the signal timers. Therefore, setStates function should be used to set the timers before using predictState ever.
+
+### OSM XML Example
+
+```(xml)
+<!-- Entry Lanelet -->
+<relation id="1349" visible="true" version="1">
+  <member type="way" ref="1347" role="left" />
+  <member type="way" ref="1348" role="right" />
+  <tag k="location" v="urban" />
+  <tag k="subtype" v="road" />
+  <tag k="type" v="lanelet" />
+
+  <!-- Carma Traffic Signal -->
+  <member type='relation' ref='45221' role='regulatory_element' />
+</relation>
+
+<!-- Exit Lanelet -->
+<relation id="1350" visible="true" version="1">
+  <member type="way" ref="1399" role="left" />
+  <member type="way" ref="1398" role="right" />
+  <tag k="location" v="urban" />
+  <tag k="subtype" v="road" />
+  <tag k="type" v="lanelet" />
+</relation>
+
+<!-- Virtual Linestring for Stop Line >
+<way id="1399" visible="false" version="1">
+  <nd ref="1339" />
+  <nd ref="1342" />
+  <tag k="subtype" v="-" />
+  <tag k="type" v="virtual" />
+</way>
+
+<!-- Regulatory Carma Traffic Signal -->
+<relation id='45221' visible='true' version='1'>
+  <member type="way" ref="1399" role="ref_line" /> <!-- Horizontal linestring representing the stop line -->
+  <member type='relation' ref="1349" role='control_start' />
+  <member type='relation' ref="1350" role='control_end' />
+  <tag k='subtype' v='carma_traffic_signal' />
+  <tag k='type' v='regulatory_element' />
+</relation>
+
+```
+
+## Signalized Intersection
+
+Represents signalized intersection on the road. 
+
+SignalizedIntersection consists of ENTRY, EXIT, INTERIOR lanelets that are saved as parameters. 
+Although this class doesn't manage it, entry lanelets are expected to have CarmaTrafficSignal class to represent the traffic light.
+Therefore, this class can be understood as merely recording of which lanelets are part of the intersection, but traffic signal timer
+or which entry correlates to which exit information is handled by each CarmaTrafficSignal object itself.
+
+### Parameters
+
+| **Role** | **Possible Type** | **description**                |
+|-------------|--------------|----------------------------------|
+| **intersection_entry**    | **Lanelet**    | A lanelet representing the entry of this intersection|
+| **intersection_exit**    | **Lanelet**    | A lanelet representing the exit of this intersection|
+| **intersection_interior**    | **Lanelet**    |  A lanelet representing the interior of this intersection|
+
+### Custom Attributes
+
+| **Key** | **Value Type** | **description**                |
+|-------------|--------------|----------------------------------|
+| **subtype** | **signalized_intersection**    | Subtype name |
+
+### OSM XML Example
+
+```(xml)
+<!-- Entry Lanelets -->
+<relation id="1349" visible="true" version="1">
+  <member type="way" ref="1347" role="left" />
+  <member type="way" ref="1348" role="right" />
+  <tag k="dynamic" v="no" />
+  <tag k="location" v="urban" />
+  <tag k="one_way" v="yes" />
+  <tag k="subtype" v="road" />
+  <tag k="type" v="lanelet" />
+
+  <!-- Signalized Intersection -->
+  <member type='relation' ref='45219' role='regulatory_element' />
+</relation>
+<relation id="1352" visible="true" version="1">
+  <member type="way" ref="1347" role="left" />
+  <member type="way" ref="1348" role="right" />
+  <tag k="dynamic" v="no" />
+  <tag k="location" v="urban" />
+  <tag k="one_way" v="yes" />
+  <tag k="subtype" v="road" />
+  <tag k="type" v="lanelet" />
+
+  <!-- Signalized Intersection -->
+  <member type='relation' ref='45219' role='regulatory_element' />
+</relation>
+
+<!-- Exit Lanelet -->
+<relation id="1350" visible="true" version="1">
+  <member type="way" ref="1347" role="left" />
+  <member type="way" ref="1348" role="right" />
+  <tag k="dynamic" v="no" />
+  <tag k="location" v="urban" />
+  <tag k="one_way" v="yes" />
+  <tag k="subtype" v="road" />
+  <tag k="type" v="lanelet" />
+</relation>
+
+<!-- Interior Lanelet -->
+<relation id="1351" visible="true" version="1">
+  <member type="way" ref="1347" role="left" />
+  <member type="way" ref="1348" role="right" />
+  <tag k="dynamic" v="no" />
+  <tag k="location" v="urban" />
+  <tag k="one_way" v="yes" />
+  <tag k="subtype" v="road" />
+  <tag k="type" v="lanelet" />
+</relation>
+
+<!-- Regulatory Signalized Intersection Rule -->
+<relation id='45219' visible='true' version='1'>
+  <member type='relation' ref='1349' role='intersection_entry' />
+  <member type='relation' ref='1350' role='intersection_exit' />
+  <member type='relation' ref='1351' role='intersection_interior' />
+  <member type='relation' ref='1352' role='intersection_entry' />
+  <tag k='subtype' v='signalized_intersection' />
+  <tag k='type' v='regulatory_element' />
+</relation>
+
+```

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

@@ -38,7 +38,7 @@ namespace lanelet
  * CAUTION_CONFLICTING_TRAFFIC : Yellow Flashing
  *
  */
-enum class CarmaTrafficLightState {
+enum class CarmaTrafficSignalState {
   UNAVAILABLE=0,
   DARK=1,
   STOP_THEN_PROCEED=2,
@@ -51,6 +51,12 @@ enum class CarmaTrafficLightState {
   CAUTION_CONFLICTING_TRAFFIC=9
 };
 
+struct CarmaTrafficSignalRoleNameString
+{
+  static constexpr char ControlStart[] = "control_start";
+  static constexpr char ControlEnd[] = "control_end";
+};
+
 // Namespace for time representations used with this regulatory element
 namespace time {
 
@@ -88,9 +94,9 @@ namespace time {
 }
 
 /**
- * \brief Stream operator for CarmaTrafficLightState enum.
+ * \brief Stream operator for CarmaTrafficSignalState enum.
  */
-std::ostream& operator<<(std::ostream& os, CarmaTrafficLightState s);
+std::ostream& operator<<(std::ostream& os, CarmaTrafficSignalState s);
 
 /**
  * @brief: Class representing a known timing traffic light.
@@ -100,58 +106,73 @@ std::ostream& operator<<(std::ostream& os, CarmaTrafficLightState s);
  * @ingroup Primitives
  */
 
-class CarmaTrafficLight : public lanelet::RegulatoryElement
+class CarmaTrafficSignal : public lanelet::RegulatoryElement
 {
 public:
-  static constexpr char RuleName[] = "carma_traffic_light";
+  static constexpr char RuleName[] = "carma_traffic_signal";
+
   int revision_ = 0; //indicates when was this last modified
   boost::posix_time::time_duration fixed_cycle_duration;
-  std::vector<std::pair<boost::posix_time::ptime, CarmaTrafficLightState>> recorded_time_stamps;
+  std::vector<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> recorded_time_stamps;
   /**
    * @brief setStates function sorts states automatically
    *
    * @param data The data to initialize this regulation with
    * NOTE: to extract full cycle, first and last state should match in input_time_steps
    */
-  void setStates(std::vector<std::pair<boost::posix_time::ptime, CarmaTrafficLightState>> input_time_steps, int revision);
+  void setStates(std::vector<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> input_time_steps, int revision);
 
   /**
-   * @brief getControlledLanelets function returns lanelets this element controls
+   * @brief getControlStartLanelets function returns lanelets where this element's control starts
+   * NOTE: Order of the lanelets does not correlate to the order of the control_end lanelets
+   */
+  lanelet::ConstLanelets getControlStartLanelets() const;
+
+  /**
+   * @brief getControlEndLanelets function returns lanelets where this element's control ends
+   * NOTE: Order of the lanelets does not correlate to the order of the control_start lanelets
+   * 
    */
-  lanelet::ConstLanelets getControlledLanelets() const;
+  lanelet::ConstLanelets getControlEndLanelets() const;
 
   /**
    * @brief prefictState assumes sorted, fixed time, so guaranteed to give you one final state
    *
    * @param time_stamp boost::posix_time::ptime of the event happening
    */
-  boost::optional<CarmaTrafficLightState> predictState(boost::posix_time::ptime time_stamp);
+  boost::optional<CarmaTrafficSignalState> predictState(boost::posix_time::ptime time_stamp);
+
+  /**
+   * @brief Return the stop_lines related to the entry lanelets in order if exists.
+   */
   ConstLineStrings3d stopLine() const;
   LineStrings3d stopLine();
 
-  explicit CarmaTrafficLight(const lanelet::RegulatoryElementDataPtr& data);
+  explicit CarmaTrafficSignal(const lanelet::RegulatoryElementDataPtr& data);
   /**
-   * @brief: Creating one is not directly usable unless setStates is called Static helper function that creates a stop line data object based on the provided inputs
+   * @brief: Creating one is not directly usable unless setStates is called 
+   *         Static helper function that creates a stop line data object based on the provided inputs
    *
    * @param id The lanelet::Id of this element
-   * @param lanelets List of lanelets this element controls.
-   * @param stop_line The line string which represent the stop line of the traffic light
+   * @param entry_lanelets List of lanelets where this element's control starts
+   * @param exit_lanelets List of lanelets where this element's control ends
+   * @param stop_lines The line strings which represent the stop lines of the entry_lanelets (in order) controlled by the traffic light
    *
    * @return RegulatoryElementData containing all the necessary information to construct a stop rule
    */
-  static std::unique_ptr<lanelet::RegulatoryElementData> buildData(Id id, LineString3d stop_line, Lanelets lanelets);
+  static std::unique_ptr<lanelet::RegulatoryElementData> buildData(Id id, LineStrings3d stop_lines, Lanelets entry_lanelets, Lanelets exit_lanelets);
 
 
 private:
   // the following lines are required so that lanelet2 can create this object
   // when loading a map with this regulatory element
-  friend class lanelet::RegisterRegulatoryElement<CarmaTrafficLight>;
+  friend class lanelet::RegisterRegulatoryElement<CarmaTrafficSignal>;
 };
 
 
 // Convenience Ptr Declarations
-using CarmaTrafficLightPtr = std::shared_ptr<CarmaTrafficLight>;
-using CarmaTrafficLightConstPtr = std::shared_ptr<const CarmaTrafficLight>;
+using CarmaTrafficSignalPtr = std::shared_ptr<CarmaTrafficSignal>;
+using CarmaTrafficSignalConstPtr = std::shared_ptr<const CarmaTrafficSignal>;
 
 }  // namespace lanelet