merge release/k900 branch to master (#246)

<!-- Thanks for the contribution, this is awesome. -->
This PR is to merge release/k900 branch to carma-master as release
process
# PR Details
## Description

<!--- Describe your changes in detail -->

## Related Issue

<!--- This project only accepts pull requests related to open issues -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please DO NOT name partially fixed issues, instead open an issue
specific to this fix -->
<!--- Please link to the issue here: -->

## Motivation and Context

<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?

<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [ ] Defect fix (non-breaking change that fixes an issue)
- [ ] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [ ] My change requires a change to the documentation.
- [ ] I have updated the documentation accordingly.
- [ ] I have read the
[**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md)
document.
- [ ] I have added tests to cover my changes.
- [ ] All new and existing tests passed.

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

Dockerfile

@@ -1,4 +1,4 @@
-FROM usdotfhwastol/carma-base:carma-system-4.3.0 AS base_image
+FROM usdotfhwastol/carma-base:carma-system-4.4.0 AS base_image
 
 FROM base_image AS build
 

common/lanelet2_extension/CMakeLists.txt

@@ -63,6 +63,7 @@ set(cpp_files
   lib/RegionAccessRule.cpp
   lib/DirectionOfTravel.cpp
   lib/TimeConversion.cpp
+  lib/BusStopRule.cpp
 )
 
 
@@ -222,6 +223,7 @@ else()
       test/src/RegionAccessRuleTest.cpp
       test/src/PassingControlLineTest.cpp
       test/src/StopRuleTest.cpp
+      test/src/BusStopRuleTest.cpp
       test/src/DirectionOfTravelTest.cpp
       test/src/DigitalSpeedLimitTest.cpp
       test/src/DigitalMinimumGapTest.cpp

common/lanelet2_extension/docs/RegulatoryElements.md

@@ -461,3 +461,55 @@ or which entry correlates to which exit information is handled by each CarmaTraf
 </relation>
 
 ```
+## Bus Stop Rule
+
+Represents a virtual bus stop and wait line horizontally laying on the roadway. It indicates whether a given participant bus 
+should stop and wait momentarily before passing the line. General usage is as a  bus stop line that is not represented by an actual
+physical roadway object. By default, it only apply to bus in the participant list.
+
+A BusStopRule is created from a list of contiguous LineString3d and participant bus which should stop and wait at bus stop before crossing.
+The object is agnostic to the line's invertedness.
+
+### Parameters
+
+| **Role** | **Possible Type** | **description**                |
+|-------------|--------------|----------------------------------|
+| **ref_line**    | **LineString3d**    | The linestrings which define the geometry of this stop line. Must be contiguous |
+
+### Custom Attributes
+
+| **Key** | **Value Type** | **description**                |
+|-------------|--------------|----------------------------------|
+| **subtype** | **stop_rule**    | Subtype name |
+
+### OSM XML Example
+
+```(xml)
+<!-- 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" />
+
+  <!-- Bus Stop Rule -->
+  <member type='relation' ref='45221' role='regulatory_element' />
+</relation>
+
+<!-- Virtual Linestring for Bus Stop Rule>
+<way id="1349" visible="false" version="1">
+  <nd ref="1339" />
+  <nd ref="1342" />
+  <tag k="subtype" v="-" />
+  <tag k="type" v="virtual" />
+</way>
+
+<!-- Regulatory Bus Stop Rule -->
+<relation id='45221' visible='true' version='1'>
+  <member type="way" ref="1349" role="ref_line" /> <!-- Horizontal linestring representing the stop line -->
+  <tag k='subtype' v='stop_rule' />
+  <tag k='type' v='regulatory_element' />
+  </relation>
+
+```

common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/BusStopRule.h

@@ -0,0 +1,68 @@
+#pragma once
+/*
+ * Copyright (C) 2023 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 <lanelet2_core/primitives/RegulatoryElement.h>
+#include <boost/algorithm/string.hpp>
+#include <unordered_set>
+#include "StopRule.h"
+
+namespace lanelet
+{
+/**
+ * @brief Represents a virtual bus stop and wait line horizontally laying on the roadway. It indicates whether a given participant bus 
+ * should stop and wait momentarily before passing the line. General usage is as a  bus stop line that is not represented by an actual
+ * physical roadway object. By default, it only apply to bus in the participant list.
+ *
+ * A BusStopRule is created from a list of contiguous LineString3d and participant bus which should stop and wait at bus stop before crossing.
+ * The object is agnostic to the line's invertedness.
+ *
+ * @ingroup RegulatoryElementPrimitives
+ * @ingroup Primitives
+ */
+class BusStopRule : public StopRule
+{
+public:
+  static constexpr char RuleName[] = "stop_rule";
+  static constexpr char Participants[] = "participants";
+  std::unordered_set<std::string> participants_ = {"bus"};
+
+  /**
+   * @brief Constructor defined to support loading from lanelet files
+   */
+  explicit BusStopRule(const lanelet::RegulatoryElementDataPtr& data);
+
+  /**
+   * @brief Static helper function that creates a stop line data object based on the provided inputs
+   *
+   * @param id The lanelet::Id of this element
+   * @param stopAndWaitLine The line strings which represents the virtual stop line before with bus will stop at a bus stop
+   * @param participants The set of participants which this rule applies to
+   *
+   * @return RegulatoryElementData containing all the necessary information to construct a stop rule
+   */
+  static std::unique_ptr<lanelet::RegulatoryElementData> buildData(Id id, LineStrings3d stopAndWaitLine);
+
+protected:
+  // the following lines are required so that lanelet2 can create this object when loading a map with this regulatory
+  // element
+  friend class RegisterRegulatoryElement<BusStopRule>;
+};
+
+// Convenience Ptr Declarations
+using BusStopRulePtr = std::shared_ptr<BusStopRule>;
+using BusStopRuleConstPtr = std::shared_ptr<const BusStopRule>;
+
+}  // namespace lanelet

common/lanelet2_extension/lib/BusStopRule.cpp

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2023 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 <lanelet2_extension/regulatory_elements/BusStopRule.h>
+#include "RegulatoryHelpers.h"
+
+namespace lanelet
+{
+// C++ 14 vs 17 parameter export
+#if __cplusplus < 201703L
+constexpr char BusStopRule::RuleName[];  // instantiate string in cpp file
+constexpr char BusStopRule::Participants[];
+#endif
+
+BusStopRule::BusStopRule(const lanelet::RegulatoryElementDataPtr& data) : StopRule(data)
+{
+  // Read participants
+  addParticipantsToSetFromMap(participants_, attributes());
+}
+
+std::unique_ptr<lanelet::RegulatoryElementData> BusStopRule::buildData(Id id, LineStrings3d stopAndWaitLine)
+{
+  for (auto ls : stopAndWaitLine)
+  {
+    if (ls.empty()) throw lanelet::InvalidInputError("Empty linestring was passed into StopRule buildData function");
+  }
+
+  // Add parameters
+  RuleParameterMap rules;
+
+  rules[lanelet::RoleNameString::RefLine].insert(rules[lanelet::RoleNameString::RefLine].end(), stopAndWaitLine.begin(),
+                                                 stopAndWaitLine.end());
+
+  // Add attributes
+  AttributeMap attribute_map({
+      { AttributeNamesString::Type, AttributeValueString::RegulatoryElement },
+      { AttributeNamesString::Subtype, RuleName },
+  });
+
+  const std::string key = "participant:vehicle:bus";
+  attribute_map[key] = "yes";
+
+  return std::make_unique<RegulatoryElementData>(id, rules, attribute_map);
+}
+
+namespace
+{
+  // this object actually does the registration work for us
+  static lanelet::RegisterRegulatoryElement<lanelet::BusStopRule> reg;
+}  // namespace
+
+}  // namespace lanelet

common/lanelet2_extension/test/src/BusStopRuleTest.cpp

@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2023 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 <gmock/gmock.h>
+#include <iostream>
+#include <lanelet2_extension/regulatory_elements/BusStopRule.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+#include <lanelet2_core/Attribute.h>
+#include "TestHelpers.h"
+
+using ::testing::_;
+using ::testing::A;
+using ::testing::DoAll;
+using ::testing::InSequence;
+using ::testing::Return;
+using ::testing::ReturnArg;
+
+namespace lanelet
+{
+
+TEST(BusStopRuleTest, BusStopRule)
+{
+  auto pl1 = carma_wm::getPoint(0, 0, 0);
+  auto pl2 = carma_wm::getPoint(0, 1, 0);
+  auto pl3 = carma_wm::getPoint(0, 2, 0);
+  auto pr1 = carma_wm::getPoint(1, 0, 0);
+  auto pr2 = carma_wm::getPoint(1, 1, 0);
+  auto pr3 = carma_wm::getPoint(1, 2, 0);
+
+  std::vector<lanelet::Point3d> left_1 = { pl1, pl2, pl3 };
+  std::vector<lanelet::Point3d> right_1 = { pr1, pr2, pr3 };
+  auto ll_1 = carma_wm::getLanelet(left_1, right_1, lanelet::AttributeValueString::SolidDashed,
+                                   lanelet::AttributeValueString::Dashed);
+  lanelet::Id stop_line_id = utils::getId();
+  LineString3d virtual_stop_line(stop_line_id, {pl2, pr2});
+  // Creat passing control line for solid dashed line
+  std::shared_ptr<BusStopRule> stop_and_wait(new BusStopRule(BusStopRule::buildData(
+      lanelet::utils::getId(), { virtual_stop_line })));
+
+  ll_1.addRegulatoryElement(stop_and_wait);
+
+  LineStrings3d nonconstLine = stop_and_wait->stopAndWaitLine();
+  ASSERT_EQ(1, nonconstLine.size());
+  ASSERT_EQ(nonconstLine.front().id(), stop_line_id);
+
+  ASSERT_TRUE(stop_and_wait->appliesTo(lanelet::Participants::VehicleBus));
+  ASSERT_FALSE(stop_and_wait->appliesTo(lanelet::Participants::Bicycle));
+
+}
+
+}  // namespace lanelet

common/lanelet2_extension/test/src/CarmaTrafficSignalTest.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2021 LEIDOS.
+ * Copyright (C) 2023 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
@@ -80,8 +80,7 @@ TEST(CarmaTrafficSignalTest, CarmaTrafficSignal)
   ASSERT_THROW(traffic_light->predictState(time::timeFromSec(1002.5)), std::invalid_argument);
   traffic_light->recorded_start_time_stamps.push_back(time::timeFromSec(1000));
   traffic_light->recorded_start_time_stamps.push_back(time::timeFromSec(1001));
-  ASSERT_EQ(traffic_light->predictState(time::timeFromSec(1011.5)).get().second, static_cast<lanelet::CarmaTrafficSignalState>(1));
-  ASSERT_EQ(traffic_light->predictState(time::timeFromSec(1011.5)).get().first, time::timeFromSec(1012));
+  ASSERT_EQ(traffic_light->predictState(time::timeFromSec(1011.5)).get().second, static_cast<lanelet::CarmaTrafficSignalState>(3));
   /// END DYNAMIC SPAT TEST
 
   input_time_steps.push_back(std::make_pair(time::timeFromSec(1003),static_cast<lanelet::CarmaTrafficSignalState>(2)));