Feature/Communicate Speed Change (#728)

carma_wm/CMakeLists.txt

@@ -56,6 +56,7 @@ add_library(
   src/WMListener.cpp
   src/WMListenerWorker.cpp
   src/Geometry.cpp
+  src/TrafficControl.cpp
   src/IndexedDistanceMap.cpp
 )
 
@@ -99,6 +100,7 @@ catkin_add_gmock(${PROJECT_NAME}-test
   test/IndexedDistanceMapTest.cpp
   test/WMListenerWorkerTest.cpp
   test/GeometryTest.cpp
+  test/TrafficControlTest.cpp
   WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests
 )
 

carma_wm/include/carma_wm/CARMAWorldModel.h

@@ -28,7 +28,7 @@
 namespace carma_wm
 {
 /*! \brief Class which implements the WorldModel interface. In addition this class provides write access to the world
- *         model. Write access is achieved through setters for the Map and Route.
+ *         model. Write access is achieved through setters for the Map and Route and getMutableMap().
  *         NOTE: This class should NOT be used in runtime code by users and is exposed solely for use in unit tests where the WMListener class cannot be instantiated. 
  *
  *  Proper usage of this class dictates that the Map and Route object be kept in sync. For this reason normal WorldModel users should not try to construct this class directly unless in unit tests.
@@ -64,6 +64,12 @@ class CARMAWorldModel : public WorldModel
    */
   void setRoute(LaneletRoutePtr route);
 
+  /*! \brief Get a mutable version of the current map
+   * 
+   *  NOTE: the user must make sure to setMap() after any edit to the map and to set a valid route
+   */
+  lanelet::LaneletMapPtr getMutableMap() const;
+
   /*! \brief Update internal records of roadway objects. These objects MUST be guaranteed to be on the road. 
    * 
    * These are detected by the sensor fusion node and are passed as objects compatible with lanelet 

carma_wm/include/carma_wm/TrafficControl.h

@@ -0,0 +1,140 @@
+#pragma once
+
+/*
+ * Copyright (C) 2020 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 <ros/ros.h>
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_io.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <lanelet2_core/LaneletMap.h>
+#include <autoware_lanelet2_msgs/MapBin.h>
+#include <lanelet2_io/io_handlers/Serialize.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <lanelet2_extension/regulatory_elements/DigitalSpeedLimit.h>
+#include <lanelet2_core/primitives/LaneletOrArea.h>
+
+
+namespace carma_wm
+{
+/*! \brief This class defines an update to traffic regulations received from carma_wm_broadcaster.
+           This class can be sent/received over ROS network in a binary ROS msg format.
+ */
+using namespace lanelet::units::literals;
+
+class TrafficControl
+{
+public:
+  TrafficControl(){}
+  TrafficControl(boost::uuids::uuid id,
+                 std::vector<std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>> update_list, 
+                 std::vector<std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>> remove_list):
+                 id_(id), update_list_(update_list), remove_list_(remove_list){}  
+
+  boost::uuids::uuid id_;  // Unique id of this geofence
+
+   // elements needed for broadcasting to the rest of map users
+  std::vector<std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>> update_list_;
+  std::vector<std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>> remove_list_;
+};
+
+/**
+ * [Converts carma_wm::TrafficControl object to ROS message. Similar implementation to 
+ * lanelet2_extension::utility::message_conversion::toBinMsg]
+ * @param gf_ptr [Ptr to Geofence data]
+ * @param msg [converted ROS message. Only "data" field is filled]
+ * NOTE: When converting the geofence object, the converter fills its relevant map update
+ * fields (update_list, remove_list) to be read once received at the user
+ */
+void toBinMsg(std::shared_ptr<carma_wm::TrafficControl> gf_ptr, autoware_lanelet2_msgs::MapBin* msg);
+
+/**
+ * [Converts Geofence binary ROS message to carma_wm::TrafficControl object. Similar implementation to 
+ * lanelet2_extension::utility::message_conversion::fromBinMsg]
+ * @param msg [ROS message for geofence]
+ * @param gf_ptr [Ptr to converted Geofence object]
+ * NOTE: When converting the geofence object, the converter only fills its relevant map update
+ * fields (update_list, remove_list) as the ROS msg doesn't hold any other data field in the object.
+ */
+void fromBinMsg(const autoware_lanelet2_msgs::MapBin& msg, std::shared_ptr<carma_wm::TrafficControl> gf_ptr);
+
+
+}  // namespace carma_wm
+
+
+// used for converting geofence into ROS msg binary
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void save(Archive& ar, const carma_wm::TrafficControl& gf, unsigned int /*version*/) 
+{
+  std::string string_id = boost::uuids::to_string(gf.id_);
+  ar << string_id;
+
+  // convert the regems that need to be removed
+  size_t remove_list_size = gf.remove_list_.size();
+  ar << remove_list_size;
+  for (auto pair : gf.remove_list_) ar << pair;
+
+  // convert id, regem pairs that need to be updated
+  size_t update_list_size = gf.update_list_.size();
+  ar << update_list_size;
+  for (auto pair : gf.update_list_) ar << pair;
+}
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void load(Archive& ar, carma_wm::TrafficControl& gf, unsigned int /*version*/) 
+{
+  boost::uuids::string_generator gen;
+  std::string id;
+  ar >> id;
+  gf.id_ = gen(id);
+
+  // save regems to remove
+  size_t remove_list_size;
+  ar >> remove_list_size;
+  for (auto i = 0u; i < remove_list_size; ++i) 
+  {
+    std::pair<lanelet::Id, lanelet::RegulatoryElementPtr> remove_item;
+    ar >> remove_item;
+    gf.remove_list_.push_back(remove_item);
+  }
+
+  // save parts that need to be updated
+  size_t update_list_size;
+  ar >> update_list_size;
+  for (auto i = 0u; i < update_list_size; ++i) 
+  {
+    std::pair<lanelet::Id, lanelet::RegulatoryElementPtr> update_item;
+    ar >> update_item;
+    gf.update_list_.push_back(update_item);
+  }
+}
+
+template <typename Archive>
+void serialize(Archive& ar, std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>& p, unsigned int /*version*/) 
+{
+  ar& p.first;
+  ar& p.second;
+}
+
+} // namespace serialization
+} // namespace boost
+
+BOOST_SERIALIZATION_SPLIT_FREE(carma_wm::TrafficControl);

carma_wm/include/carma_wm/WMListener.h

@@ -22,6 +22,7 @@
 #include <ros/callback_queue.h>
 #include <carma_wm/WorldModel.h>
 #include <carma_utils/CARMAUtils.h>
+#include <autoware_lanelet2_msgs/MapBin.h>
 
 namespace carma_wm
 {
@@ -94,7 +95,10 @@ class WMListener
   std::unique_lock<std::mutex> getLock(bool pre_locked = true);
 
 private:
+  // Callback function that uses lock to edit the map
+  void mapUpdateCallback(const autoware_lanelet2_msgs::MapBinConstPtr& geofence_msg);
   ros::Subscriber roadway_objects_sub_;
+  ros::Subscriber map_update_sub_;
   std::unique_ptr<WMListenerWorker> worker_;
   ros::CARMANodeHandle nh_;
   ros::CallbackQueue async_queue_;

carma_wm/src/CARMAWorldModel.cpp

@@ -278,6 +278,11 @@ void CARMAWorldModel::setMap(lanelet::LaneletMapPtr map)
   map_routing_graph_ = std::move(map_graph);
 }
 
+lanelet::LaneletMapPtr CARMAWorldModel::getMutableMap() const
+{
+  return semantic_map_;
+}
+
 void CARMAWorldModel::setRoute(LaneletRoutePtr route)
 {
   route_ = route;

carma_wm/src/TrafficControl.cpp

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2020 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/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+#include <carma_wm/TrafficControl.h>
+
+namespace carma_wm
+{
+
+void toBinMsg(std::shared_ptr<carma_wm::TrafficControl> gf_ptr, autoware_lanelet2_msgs::MapBin* msg)
+{
+  if (msg == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": msg is null pointer!");
+    return;
+  }
+  std::stringstream ss;
+  boost::archive::binary_oarchive oa(ss);
+  oa << *gf_ptr;
+  std::string data_str(ss.str());
+
+  msg->data.clear();
+  msg->data.assign(data_str.begin(), data_str.end());
+}
+
+void fromBinMsg(const autoware_lanelet2_msgs::MapBin& msg, std::shared_ptr<carma_wm::TrafficControl> gf_ptr)
+{
+  if (!gf_ptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": gf_ptr is null pointer!");
+    return;
+  }
+
+  std::string data_str;
+  data_str.assign(msg.data.begin(), msg.data.end());
+
+  std::stringstream ss;
+  ss << data_str;
+  boost::archive::binary_iarchive oa(ss);
+  oa >> *gf_ptr;
+}
+
+}  // namespace carma_wm

carma_wm/src/WMListener.cpp

@@ -30,7 +30,7 @@ WMListener::WMListener(bool multi_thread) : worker_(std::unique_ptr<WMListenerWo
     ROS_DEBUG_STREAM("WMListener: Using multi-threaded subscription");
     nh_.setCallbackQueue(&async_queue_);
   }
-
+  map_update_sub_= nh_.subscribe("map_update", 1, &WMListener::mapUpdateCallback, this);
   map_sub_ = nh_.subscribe("semantic_map", 1, &WMListenerWorker::mapCallback, worker_.get());
   // route_sub_ = nh_.subscribe("route", 1, &WMListenerWorker::routeCallback, worker_.get()); // TODO uncomment when
   // route message is defined
@@ -57,6 +57,12 @@ WorldModelConstPtr WMListener::getWorldModel()
   return worker_->getWorldModel();
 }
 
+void WMListener::mapUpdateCallback(const autoware_lanelet2_msgs::MapBinConstPtr& geofence_msg)
+{
+  const std::lock_guard<std::mutex> lock(mw_mutex_);
+  worker_->mapUpdateCallback(geofence_msg);
+}
+
 void WMListener::setMapCallback(std::function<void()> callback)
 {
   const std::lock_guard<std::mutex> lock(mw_mutex_);

carma_wm/src/WMListenerWorker.cpp

@@ -43,6 +43,48 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::MapBinConstPtr&
     map_callback_();
   }
 }
+void WMListenerWorker::mapUpdateCallback(const autoware_lanelet2_msgs::MapBinConstPtr& geofence_msg) const
+{
+  // convert ros msg to geofence object
+  auto gf_ptr = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl());
+  carma_wm::fromBinMsg(*geofence_msg, gf_ptr);
+  ROS_INFO_STREAM("New Map Update Received with Geofence Id:" << gf_ptr->id_);
+
+  ROS_INFO_STREAM("Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size());
+  for (auto pair : gf_ptr->remove_list_)
+  {
+    auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first);
+    // we can only check by id, if the element is there
+    // this is only for speed optimization, as world model here should blindly accept the map update received
+    for (auto regem: parent_llt.regulatoryElements())
+    {
+      // we can't use the deserialized element as its data address conflicts the one in this node
+      if (pair.second->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem);
+    }
+  }
+
+  ROS_INFO_STREAM("Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size());
+  for (auto pair : gf_ptr->update_list_)
+  {
+    auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first);
+    auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(pair.second->id());
+    // if this regem is already in the map
+    if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end())
+    {
+      // again we should use the element with correct data address to be consistent
+      world_model_->getMutableMap()->update(parent_llt, *regemptr_it);
+    }
+    else
+    {
+      world_model_->getMutableMap()->update(parent_llt, pair.second);
+    }
+  }
+
+  // set the map to set a new routing
+  world_model_->setMap(world_model_->getMutableMap());
+  ROS_INFO_STREAM("Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_);
+}
+
 
 void WMListenerWorker::roadwayObjectListCallback(const cav_msgs::RoadwayObstacleList& msg)
 {

carma_wm/src/WMListenerWorker.h

@@ -18,6 +18,7 @@
 
 #include <autoware_lanelet2_msgs/MapBin.h>
 #include <carma_wm/CARMAWorldModel.h>
+#include <carma_wm/TrafficControl.h>
 
 namespace carma_wm
 {
@@ -41,10 +42,17 @@ class WMListenerWorker
   /*!
    * \brief Callback for new map messages. Updates the underlying map
    *
-   * \param map_msg The new map messaged to generate the map from
+   * \param map_msg The new map messages to generate the map from
    */
   void mapCallback(const autoware_lanelet2_msgs::MapBinConstPtr& map_msg);
 
+  /*!
+   * \brief Callback for new map update messages (geofence). Updates the underlying map
+   *
+   * \param geofence_msg The new map update messages to generate the map edits from
+   */
+  void mapUpdateCallback(const autoware_lanelet2_msgs::MapBinConstPtr& geofence_msg) const;
+
   /*!
    * \brief Callback for route message. It is a TODO: To update function when route message spec is defined
    */

carma_wm/test/TestHelpers.h

@@ -159,4 +159,5 @@ inline void addDisjointRoute(CARMAWorldModel& cmw)
   cmw.setRoute(route_ptr);
   cmw.setMap(map);
 }
+
 }  // namespace carma_wm