Merge pull request #176 from usdot-fhwa-stol/release/a1

Merge release/a1 branch to master for 3.8.0

.circleci/config.yml

@@ -28,7 +28,7 @@ jobs:
     # Pull docker image from docker hub
     # XTERM used for better catkin_make output
     docker:
-      - image: usdotfhwastoldev/carma-base:develop
+      - image: usdotfhwastol/carma-base:carma-system-3.8.0
         user: carma
         environment:
           TERM: xterm

Dockerfile

@@ -1,4 +1,4 @@
-FROM usdotfhwastoldev/carma-base:develop as base_image
+FROM usdotfhwastol/carma-base:carma-system-3.8.0 as base_image
 
 FROM base_image as build
 

common/lanelet2_extension/CMakeLists.txt

@@ -77,6 +77,7 @@ add_library( lanelet2_extension_lib
   lib/utilities.cpp
   lib/visualization.cpp
   lib/CarmaUSTrafficRules.cpp
+  lib/CarmaTrafficLight.cpp
   lib/PassingControlLine.cpp
   lib/StopRule.cpp
   lib/DigitalSpeedLimit.cpp
@@ -144,6 +145,7 @@ if(CATKIN_ENABLE_TESTING)
   test/src/CarmaUSTrafficRulesTest.cpp
   test/src/MapLoadingTest.cpp
   test/src/test_local_projector.cpp
+  test/src/CarmaTrafficTest.cpp
   WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests
  )
 

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

@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2021 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.
+ */
+
+#ifndef LANELET2_EXTENSION_REGULATORY_ELEMENTS_CARMA_TRAFFIC_LIGHT_H
+#define LANELET2_EXTENSION_REGULATORY_ELEMENTS_CARMA_TRAFFIC_LIGHT_H
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+#include <ros/ros.h>
+
+namespace lanelet
+{
+/**
+ * @brief: Enum representing Traffic Light States. 
+ * These states match the SAE J2735 PhaseState definitions used for SPaT messages
+ * 
+ * UNAVAILABLE : No data available
+ * DARK : Light is non-functional
+ * STOP_THEN_PROCEED : Flashing Red
+ * STOP_AND_REMAIN : Solid Red
+ * PRE_MOVEMENT : Yellow to Red transition (Found only in the EU)
+ * PERMISSIVE_MOVEMENT_ALLOWED : Solid Green there could be conflict traffic
+ * PROTECTED_MOVEMENT_ALLOWED : Solid Green no chance of conflict traffic (normally used with arrows)
+ * PERMISSIVE_CLEARANCE : Yellow Solid there is a chance of conflicting traffic
+ * PROTECTED_CLEARANCE : Yellow Solid no chance of conflicting traffic (normally used with arrows)
+ * CAUTION_CONFLICTING_TRAFFIC : Yellow Flashing
+ *
+ */
+enum class CarmaTrafficLightState {
+  UNAVAILABLE=0,
+  DARK=1,
+  STOP_THEN_PROCEED=2,
+  STOP_AND_REMAIN=3,
+  PRE_MOVEMENT=4,
+  PERMISSIVE_MOVEMENT_ALLOWED=5,
+  PROTECTED_MOVEMENT_ALLOWED=6,
+  PERMISSIVE_CLEARANCE=7,
+  PROTECTED_CLEARANCE=8,
+  CAUTION_CONFLICTING_TRAFFIC=9
+};
+
+/**
+ * \brief Stream operator for CarmaTrafficLightState enum.
+ */
+std::ostream& operator<<(std::ostream& os, CarmaTrafficLightState s);
+
+/**
+ * @brief: Class representing a known timing traffic light.
+ *         Normally the traffic light timing information is provided by SAE J2735 SPaT messages although alternative data sources can be supported
+ * 
+ * @ingroup RegulatoryElementPrimitives
+ * @ingroup Primitives
+ */
+
+class CarmaTrafficLight : public lanelet::RegulatoryElement
+{
+public:
+  static constexpr char RuleName[] = "carma_traffic_light";
+  int revision_ = 0; //indicates when was this last modified
+  ros::Duration fixed_cycle_duration;
+  std::vector<std::pair<ros::Time, CarmaTrafficLightState>> 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<ros::Time, CarmaTrafficLightState>> input_time_steps, int revision);
+
+  /**
+   * @brief getControlledLanelets function returns lanelets this element controls
+   */
+  lanelet::ConstLanelets getControlledLanelets() const;
+  /**
+   * @brief getState get the current state
+   *
+   * @return return current, ros::Time::now
+   */
+  boost::optional<CarmaTrafficLightState> getState();
+  /**
+   * @brief prefictState assumes sorted, fixed time, so guaranteed to give you one final state
+   *
+   * @param time_stamp ros::Time of the event happening
+   */
+  boost::optional<CarmaTrafficLightState> predictState(ros::Time time_stamp);
+  ConstLineStrings3d stopLine() const;
+  LineStrings3d stopLine();
+
+  explicit CarmaTrafficLight(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
+   *
+   * @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
+   *
+   * @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);
+
+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>;
+};
+
+
+// Convenience Ptr Declarations
+using CarmaTrafficLightPtr = std::shared_ptr<CarmaTrafficLight>;
+using CarmaTrafficLightConstPtr = std::shared_ptr<const CarmaTrafficLight>;
+
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_REGULATORY_ELEMENTS_CARMA_TRAFFIC_LIGHT_H

common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/DigitalSpeedLimit.h

@@ -38,6 +38,9 @@ class DigitalSpeedLimit : public RegulatoryElement
 public:
   static constexpr char RuleName[] = "digital_speed_limit";
   static constexpr char Limit[] = "limit";
+  static constexpr char Reason[] = "reason";
+
+  std::string reason_ = "NA";
   Velocity speed_limit_ = 0;
   std::unordered_set<std::string> participants_;
 
@@ -62,6 +65,13 @@ class DigitalSpeedLimit : public RegulatoryElement
    */
   Velocity getSpeedLimit() const;
 
+   /**
+   * @brief Returns the reason for this rule
+   *
+   * @return The reason for this rule
+   */
+  std::string getReason();
+
   /**
    * @brief Returns true if the given participant must follow this speed limit
    *
@@ -78,11 +88,12 @@ class DigitalSpeedLimit : public RegulatoryElement
    * @param lanelets The lanelets this speed limit applies to
    * @param areas The areas this speed limit applies to
    * @param participants The set of participants which this speed limit will apply to
+   * @param reason The reason
    *
    * @return RegulatoryElementData containing all the necessary information to construct a speed limit element
    */
   static std::unique_ptr<lanelet::RegulatoryElementData> buildData(Id id, Velocity speed_limit, Lanelets lanelets, Areas areas,
-                                                     std::vector<std::string> participants);
+                                                     std::vector<std::string> participants, const std::string& reason="NA");
 
   /**
    * @brief Constructor required for compatability with lanlet2 loading

common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/RegionAccessRule.h

@@ -80,6 +80,7 @@ class RegionAccessRule : public RegulatoryElement
    * @param lanelets The lanelets impacted by this regulation
    * @param areas The areas impacted by this regulation
    * @param participants The participants which can access the provided lanelets and areas
+   * @param reason The reason
    *
    * @return RegulatoryElementData containing all the necessary information to construct a region access rule object
    */

common/lanelet2_extension/lib/CarmaTrafficLight.cpp

@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) 2021 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 <ostream>
+#include <lanelet2_extension/regulatory_elements/CarmaTrafficLight.h>
+#include "RegulatoryHelpers.h"
+
+namespace lanelet
+{
+// C++ 14 vs 17 parameter export
+#if __cplusplus < 201703L
+constexpr char CarmaTrafficLight::RuleName[];  // instantiate string in cpp file
+#endif
+
+std::ostream& operator<<(std::ostream& os, CarmaTrafficLightState s)
+{
+  switch (s)
+  {  // clang-format off
+    case CarmaTrafficLightState::UNAVAILABLE   : os << "CarmaTrafficLightState::UNAVAILABLE"; break;
+    case CarmaTrafficLightState::DARK: os << "CarmaTrafficLightState::DARK"; break;
+    case CarmaTrafficLightState::STOP_THEN_PROCEED : os << "CarmaTrafficLightState::STOP_THEN_PROCEED"; break;
+    case CarmaTrafficLightState::STOP_AND_REMAIN  : os << "CarmaTrafficLightState::STOP_AND_REMAIN"; break;
+    case CarmaTrafficLightState::PRE_MOVEMENT  : os << "CarmaTrafficLightState::PRE_MOVEMENT"; break;
+    case CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED  : os << "CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED"; break;
+    case CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED  : os << "CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED"; break;
+    case CarmaTrafficLightState::PERMISSIVE_CLEARANCE  : os << "CarmaTrafficLightState::PERMISSIVE_CLEARANCE"; break;
+    case CarmaTrafficLightState::PROTECTED_CLEARANCE  : os << "CarmaTrafficLightState::PROTECTED_CLEARANCE"; break;
+    case CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC  : os << "CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC"; break;
+    default: os.setstate(std::ios_base::failbit);
+  }  // clang-format on
+  return os;
+}
+
+ConstLineStrings3d CarmaTrafficLight::stopLine() const
+{
+  return getParameters<ConstLineString3d>(RoleName::RefLine);
+}
+
+LineStrings3d CarmaTrafficLight::stopLine()
+{
+  return getParameters<LineString3d>(RoleName::RefLine);
+}
+
+CarmaTrafficLight::CarmaTrafficLight(const lanelet::RegulatoryElementDataPtr& data) : RegulatoryElement(data)
+{}
+
+std::unique_ptr<lanelet::RegulatoryElementData> CarmaTrafficLight::buildData(Id id, LineString3d stop_line, Lanelets lanelets)
+{
+
+  if (stop_line.empty()) throw lanelet::InvalidInputError("Empty linestring was passed into CarmaTrafficLight buildData function");
+  // Add parameters
+  RuleParameterMap rules;
+  rules[lanelet::RoleNameString::Refers].insert(rules[lanelet::RoleNameString::Refers].end(), lanelets.begin(),
+                                                lanelets.end());
+  rules[lanelet::RoleNameString::RefLine].insert(rules[lanelet::RoleNameString::RefLine].end(), stop_line);
+
+  // Add attributes
+  AttributeMap attribute_map({
+      { AttributeNamesString::Type, AttributeValueString::RegulatoryElement },
+      { AttributeNamesString::Subtype, RuleName },
+  });
+
+  return std::make_unique<RegulatoryElementData>(id, rules, attribute_map);
+}
+
+boost::optional<CarmaTrafficLightState> CarmaTrafficLight::getState()
+{
+  return predictState(ros::Time::now());
+}
+
+boost::optional<CarmaTrafficLightState> CarmaTrafficLight::predictState(ros::Time time_stamp)
+{
+  if (recorded_time_stamps.empty())
+  {
+    ROS_WARN_STREAM("CarmaTrafficLight doesn't have any recorded states of traffic lights");
+    return boost::none;
+  }
+  if (recorded_time_stamps.size() == 1) // if only 1 timestamp recorded, this signal doesn't change
+  {
+    return recorded_time_stamps.front().second;
+  }
+  // shift starting time to the future or to the past to fit input into a valid cycle
+  ros::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((recorded_time_stamps.front().first - time_stamp).toSec()/ fixed_cycle_duration.toSec());
+  accumulated_offset_duration = ros::Duration( num_of_cycles * fixed_cycle_duration.toSec());
+
+  if (offset_duration_dir < 0) 
+  {
+    while (recorded_time_stamps.front().first - accumulated_offset_duration > time_stamp)
+    {
+      accumulated_offset_duration += fixed_cycle_duration;
+    }
+  }
+  // iterate through states in the cycle to get the signal
+  for (size_t i = 0; i < recorded_time_stamps.size(); i++)
+  {
+    if (recorded_time_stamps[i].first.toSec() + offset_duration_dir * accumulated_offset_duration.toSec() >= time_stamp.toSec())
+    { 
+      return recorded_time_stamps[i].second;
+    }
+  }
+
+  throw lanelet::InvalidInputError("Reached unreachable code block. Implies duplicate phase is not provided. Unable to determine fixed cycle duration");
+}
+
+lanelet::ConstLanelets CarmaTrafficLight::getControlledLanelets() const
+{
+  return getParameters<lanelet::ConstLanelet>(RoleName::Refers);
+} 
+
+void CarmaTrafficLight::setStates(std::vector<std::pair<ros::Time, CarmaTrafficLightState>> input_time_steps, int revision)
+{
+  if (input_time_steps.empty())
+  {
+    ROS_ERROR_STREAM("Given states for the CarmaTrafficLight Id: " << id() << " is empty. Returning...");
+    return;
+  }
+
+  std::sort(input_time_steps.begin(), input_time_steps.end());
+
+  //extract a cycle and trim the rest from the states
+  if (input_time_steps.size() > 2)
+  {
+    int idx = 2;
+    while (idx + 1 < input_time_steps.size() && input_time_steps[idx].second != input_time_steps[0].second && input_time_steps[idx + 1].second != input_time_steps[1].second)
+    {
+      idx ++;
+    }
+    input_time_steps.resize(idx + 1);
+  }
+  // throw where the duplicate phase is not provided
+  if (input_time_steps.back().second != input_time_steps.front().second)
+  {
+    throw lanelet::InvalidInputError("Duplicate phase is not provided. Unable to determine fixed cycle duration");
+  }
+
+  recorded_time_stamps = input_time_steps;
+  fixed_cycle_duration = recorded_time_stamps.back().first - recorded_time_stamps.front().first; // it is okay if size is only 1, case is handled in predictState
+  revision_ = revision;
+}
+namespace
+{
+// this object actually does the registration work for us
+lanelet::RegisterRegulatoryElement<lanelet::CarmaTrafficLight> reg;
+}  // namespace
+
+}  // namespace lanelet