Feature/time sync (#2216)

carma_wm/include/carma_wm/CARMAWorldModel.hpp

@@ -31,13 +31,13 @@
 #include <lanelet2_extension/time/TimeConversion.h>
 #include "boost/date_time/posix_time/posix_time.hpp"
 #include "carma_wm/SignalizedIntersectionManager.hpp"
-
+#include <rosgraph_msgs/msg/clock.hpp>
 
 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 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. 
+ *         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.
  *
@@ -71,15 +71,15 @@ class CARMAWorldModel : public WorldModel
   /*!
    * \brief Set the routing graph for the participant type.
    *        This function may serve as an optimization to recomputing the routing graph when it is already available
-   * 
-   * NOTE: The set graph will be overwritten if setMap(recompute_routing_graph=true) is called. 
+   *
+   * NOTE: The set graph will be overwritten if setMap(recompute_routing_graph=true) is called.
    *       It will not, be overwritten if the map is set with recompute_routing_graph=false
    *
-   * \param graph The graph to set. 
+   * \param graph The graph to set.
    *              NOTE: This graph must be for the participant type specified getVehicleParticipationType().
-   *              There is no way to validate this from the object so the user must ensure consistency. 
-   * 
-   */ 
+   *              There is no way to validate this from the object so the user must ensure consistency.
+   *
+   */
   void setRoutingGraph(LaneletRoutingGraphPtr graph);
 
   /*! \brief Set the current route. This route must match the current map for this class to function properly
@@ -95,14 +95,14 @@ class CARMAWorldModel : public WorldModel
   void setTrafficLightIds(uint32_t id, lanelet::Id lanelet_id);
 
   /*! \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 
+  /*! \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
    */
   void setRoadwayObjects(const std::vector<carma_perception_msgs::msg::RoadwayObstacle>& rw_objs);
 
@@ -115,11 +115,11 @@ class CARMAWorldModel : public WorldModel
   void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false);
 
   /**
-   * \brief This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane. 
+   * \brief This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane.
    * Gets Downtrack distance to AND copy of the closest object on the same lane as the given point. Also returns crosstrack
    * distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack
    * if the object is to the right relative to the reference line that crosses given object_center and is parallel to the
-   * centerline of the lane. 
+   * centerline of the lane.
    *
    * \param object_center the point to measure the distance from
    * \param section either of LANE_AHEAD, LANE_BEHIND, LANE_FULL each including the current lanelet
@@ -137,7 +137,7 @@ class CARMAWorldModel : public WorldModel
     * \param moy value of the minute of the year
    */
   boost::posix_time::ptime min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy=0, bool is_simulation = false);
-  
+
 /** \param config_lim the configurable speed limit value populated from WMListener using the config_speed_limit parameter
  * in VehicleConfigParams.yaml
 *
@@ -147,7 +147,7 @@ class CARMAWorldModel : public WorldModel
   /*! \brief Set vehicle participation type
    */
   void setVehicleParticipationType(const std::string& participant);
-  
+
   /*! \brief Get vehicle participation type
    */
   std::string getVehicleParticipationType();
@@ -159,7 +159,15 @@ class CARMAWorldModel : public WorldModel
   /*! \brief Set the name of the route
    */
   void setRouteName(const std::string& route_name);
-
+
+  /*! \brief Set current Ros1 clock (only used in simulation runs)
+   */
+  void setRos1Clock(rclcpp::Time time_now);
+
+  /*! \brief Set simulation clock clock (only used in simulation runs)
+   */
+  void setSimulationClock(rclcpp::Time time_now);
+
   /*! \brief helper for traffic signal Id
    */
   lanelet::Id getTrafficSignalId(uint16_t intersection_id,uint8_t signal_id);
@@ -169,7 +177,7 @@ class CARMAWorldModel : public WorldModel
   lanelet::CarmaTrafficSignalPtr getTrafficSignal(const lanelet::Id& id) const;
 
   /**
-   * \brief (non-const version) Gets the underlying lanelet, given the cartesian point on the map 
+   * \brief (non-const version) Gets the underlying lanelet, given the cartesian point on the map
    *
    * \param point         Cartesian point to check the corressponding lanelet
    * \param n             Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.
@@ -186,7 +194,7 @@ class CARMAWorldModel : public WorldModel
    *
    * \param point         Cartesian point to check the corressponding lanelet
    * \param n             Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.
-   * 
+   *
    * \throw std::invalid_argument if the map is not set, contains no lanelets, or if adjacent lanelet is not opposite direction
    * NOTE:  Only to be used on 2 lane, opposite direction road. Number of points in all linestrings are assumed to be roughly the same.
    *        The point is assumed to be on roughly similar shape of overlapping lanelets if any
@@ -218,7 +226,7 @@ class CARMAWorldModel : public WorldModel
   TrackPos getRouteEndTrackPos() const override;
 
   LaneletRoutingGraphConstPtr getMapRoutingGraph() const override;
-  
+
   lanelet::Optional<TrafficRulesConstPtr>
   getTrafficRules(const std::string& participant) const override;
 
@@ -250,9 +258,9 @@ class CARMAWorldModel : public WorldModel
   std::vector<lanelet::CarmaTrafficSignalPtr> getSignalsAlongRoute(const lanelet::BasicPoint2d& loc) const override;
 
   std::vector<lanelet::BusStopRulePtr> getBusStopsAlongRoute(const lanelet::BasicPoint2d& loc) const override;
-  
+
   boost::optional<std::pair<lanelet::ConstLanelet, lanelet::ConstLanelet>> getEntryExitOfSignalAlongRoute(const lanelet::CarmaTrafficSignalPtr& traffic_signal) const override;
-  
+
   std::vector<std::shared_ptr<lanelet::AllWayStop>> getIntersectionsAlongRoute(const lanelet::BasicPoint2d& loc) const override;
 
   std::vector<lanelet::SignalizedIntersectionPtr> getSignalizedIntersectionsAlongRoute(const lanelet::BasicPoint2d &loc) const;
@@ -266,7 +274,7 @@ class CARMAWorldModel : public WorldModel
   double config_speed_limit_;
 
   std::string participant_type_ = lanelet::Participants::Vehicle;
-  
+
   /*! \brief Helper function to compute the geometry of the route downtrack/crosstrack reference line
    *         This function should generally only be called from inside the setRoute function as it uses member variables
    * set in that function
@@ -285,6 +293,9 @@ class CARMAWorldModel : public WorldModel
    */
   lanelet::LineString3d copyConstructLineString(const lanelet::ConstLineString3d& line) const;
 
+  std::optional<rclcpp::Time> ros1_clock_ = std::nullopt;
+  std::optional<rclcpp::Time> simulation_clock_ = std::nullopt;
+
   std::shared_ptr<lanelet::LaneletMap> semantic_map_;
   LaneletRoutePtr route_;
   LaneletRoutingGraphPtr map_routing_graph_;
@@ -296,18 +307,18 @@ class CARMAWorldModel : public WorldModel
   IndexedDistanceMap shortest_path_distance_map_;
   lanelet::LaneletMapUPtr shortest_path_filtered_centerline_view_;  // Lanelet map view of shortest path center lines
                                                                     // only
-  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects_; // 
+  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects_; //
 
   size_t map_version_ = 0; // The current map version. This is cached from calls to setMap();
 
   std::string route_name_; // The current route name. This is set from calls to setRouteName();
-  
-  // The following constants are default timining plans for recieved traffic lights. 
+
+  // The following constants are default timining plans for recieved traffic lights.
   // The light is assumed to use these values until otherwise known
   // TODO can these be optional parameters?
   static constexpr double RED_LIGHT_DURATION = 20.0; //in sec
   static constexpr double YELLOW_LIGHT_DURATION = 3.0; //in sec
   static constexpr double GREEN_LIGHT_DURATION = 20.0; //in sec
-  
+
 };
 }  // namespace carma_wm

carma_wm/include/carma_wm/WMListener.hpp

@@ -27,6 +27,7 @@
 #include <carma_planning_msgs/msg/route.hpp>
 #include <carma_v2x_msgs/msg/spat.hpp>
 #include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
 
 namespace carma_wm
 {
@@ -50,7 +51,7 @@ class WMListener
    *
    * By default this object follows node threading behavior (ie. waiting for ros::spin())
    * If the object is operating in multi-threaded mode a ros::AsyncSpinner is used to implement a background thread.
-   * 
+   *
    * \param multi_thread If true this object will subscribe using background threads. Defaults to false
    */
   WMListener(
@@ -113,15 +114,15 @@ class WMListener
   /*!
    * \brief Use to allow updates to occur even if they invalidate the current route.
    *        This is only meant to be used by components which generate the route
-   */  
+   */
   void enableUpdatesWithoutRouteWL();
 
   /*!
    * \brief Returns true if a map update has been processed which requires rerouting.
-   *        This method is meant to be used by routing components. 
-   * 
+   *        This method is meant to be used by routing components.
+   *
    * \return True is rerouting is needed
-   */ 
+   */
   bool checkIfReRoutingNeededWL();
 
 
@@ -137,13 +138,15 @@ class WMListener
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_;
 
   std::unique_ptr<WMListenerWorker> worker_;
-  
+
   carma_ros2_utils::SubPtr<autoware_lanelet2_msgs::msg::MapBin> map_sub_;
   carma_ros2_utils::SubPtr<carma_planning_msgs::msg::Route> route_sub_;
   carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::SPAT> traffic_spat_sub_;
+  carma_ros2_utils::SubPtr<rosgraph_msgs::msg::Clock> sim_clock_sub_;
+  carma_ros2_utils::SubPtr<rosgraph_msgs::msg::Clock> ros1_clock_sub_;
   const bool multi_threaded_;
   std::mutex mw_mutex_;
-  
+
 
 };
 }  // namespace carma_wm

carma_wm/package.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<!--  
+<!--
  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
@@ -21,7 +21,7 @@
   <maintainer email="[email protected]">carma</maintainer>
 
   <license>Apache 2.0</license>
-  
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>carma_cmake_common</build_depend>
   <build_depend>ament_auto_cmake</build_depend>
@@ -42,9 +42,10 @@
   <depend>carma_v2x_msgs</depend>
   <depend>j2735_v2x_msgs</depend>
   <depend>carma_debug_ros2_msgs</depend>
+  <depend>rosgraph_msgs</depend>
+
+  <depend>autoware_lanelet2_ros_interface</depend>
 
-  <depend>autoware_lanelet2_ros_interface</depend>  
-
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

carma_wm/src/CARMAWorldModel.cpp

@@ -613,6 +613,16 @@ namespace carma_wm
     return semantic_map_;
   }
 
+  void CARMAWorldModel::setRos1Clock(rclcpp::Time time_now)
+  {
+    ros1_clock_ = time_now;
+  }
+
+  void CARMAWorldModel::setSimulationClock(rclcpp::Time time_now)
+  {
+    simulation_clock_ = time_now;
+  }
+
   void CARMAWorldModel::setRoute(LaneletRoutePtr route)
   {
     route_ = route;
@@ -1419,6 +1429,18 @@ namespace carma_wm
 
   boost::posix_time::ptime CARMAWorldModel::min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy, bool is_simulation)
   {
+    double simulation_time_difference_in_seconds = 0.0;
+
+    // NOTE: In simulation, ROS1 clock (often coming from CARLA) can have a large time ahead.
+    // the timing calculated here is in Simulation time, which is behind. Therefore, the world model adds the offset to make it meaningful to carma-platform:
+    // https://github.com/usdot-fhwa-stol/carma-platform/issues/2217
+    if (is_simulation && ros1_clock_.has_value() && simulation_clock_.has_value())
+    {
+      simulation_time_difference_in_seconds = ros1_clock_.value().seconds() - simulation_clock_.value().seconds();
+    }
+
+    min_end_time += lanelet::time::durationFromSec(simulation_time_difference_in_seconds);
+
     if (moy_exists) //account for minute of the year
     {
       auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch

carma_wm/src/WMListener.cpp

@@ -52,12 +52,12 @@ WMListener::WMListener(
     rclcpp::ParameterValue use_sim_time_param_value;
     use_sim_time_param_value = node_params_->declare_parameter("use_sim_time", rclcpp::ParameterValue (false));
   }
-  
+
   // Get params
   config_speed_limit_param = node_params_->get_parameter("config_speed_limit");
   participant_param = node_params_->get_parameter("vehicle_participant_type");
   use_sim_time_param = node_params_->get_parameter("use_sim_time");
-  
+
 
   RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded config speed limit: " << config_speed_limit_param.as_double());
   RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded vehicle participant type: " << participant_param.as_string());
@@ -73,6 +73,8 @@ WMListener::WMListener(
   rclcpp::SubscriptionOptions route_options;
   rclcpp::SubscriptionOptions roadway_objects_options;
   rclcpp::SubscriptionOptions traffic_spat_options;
+  rclcpp::SubscriptionOptions ros1_clock_options;
+  rclcpp::SubscriptionOptions sim_clock_options;
 
   if(multi_threaded_)
   {
@@ -85,25 +87,37 @@ WMListener::WMListener(
     map_options.callback_group = map_cb_group;
 
     auto route_cb_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-    route_options.callback_group = route_cb_group;                               
+    route_options.callback_group = route_cb_group;
 
     auto roadway_objects_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-    roadway_objects_options.callback_group = roadway_objects_group;                                      
+    roadway_objects_options.callback_group = roadway_objects_group;
 
     auto traffic_spat_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-    traffic_spat_options.callback_group = traffic_spat_group;  
+    traffic_spat_options.callback_group = traffic_spat_group;
 
-  }  
+    auto ros1_clock_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    ros1_clock_options.callback_group = ros1_clock_group;
+
+    auto sim_clock_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    sim_clock_options.callback_group = sim_clock_group;
+
+  }
 
   // Setup subscribers
-  route_sub_ = rclcpp::create_subscription<carma_planning_msgs::msg::Route>(node_topics_, "route", 1, 
+  route_sub_ = rclcpp::create_subscription<carma_planning_msgs::msg::Route>(node_topics_, "route", 1,
                                   std::bind(&WMListenerWorker::routeCallback, worker_.get(), std::placeholders::_1), route_options);
 
+  ros1_clock_sub_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(node_topics_, "/clock", 1,
+                                  std::bind(&WMListenerWorker::ros1ClockCallback, worker_.get(), std::placeholders::_1), ros1_clock_options);
+
+  sim_clock_sub_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(node_topics_, "/sim_clock", 1,
+                                  std::bind(&WMListenerWorker::simClockCallback, worker_.get(), std::placeholders::_1), sim_clock_options);
+
   roadway_objects_sub_ = rclcpp::create_subscription<carma_perception_msgs::msg::RoadwayObstacleList>(node_topics_, "roadway_objects", 1,
                                   std::bind(&WMListenerWorker::roadwayObjectListCallback, worker_.get(), std::placeholders::_1), roadway_objects_options);
 
   traffic_spat_sub_ = rclcpp::create_subscription<carma_v2x_msgs::msg::SPAT>(node_topics_, "incoming_spat", 1,
-                                  std::bind(&WMListenerWorker::incomingSpatCallback, worker_.get(), std::placeholders::_1), traffic_spat_options); 
+                                  std::bind(&WMListenerWorker::incomingSpatCallback, worker_.get(), std::placeholders::_1), traffic_spat_options);
 
   // NOTE: Currently, intra-process comms must be disabled for subscribers that are transient_local: https://github.com/ros2/rclcpp/issues/1753
   map_update_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the map update subscriber
@@ -112,7 +126,7 @@ WMListener::WMListener(
                                          // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner.
 
   // Create map update subscriber that will receive earlier messages that were missed ONLY if the publisher is transient_local too
-  map_update_sub_ = rclcpp::create_subscription<autoware_lanelet2_msgs::msg::MapBin>(node_topics_, "map_update", map_update_sub_qos, 
+  map_update_sub_ = rclcpp::create_subscription<autoware_lanelet2_msgs::msg::MapBin>(node_topics_, "map_update", map_update_sub_qos,
                   std::bind(&WMListener::mapUpdateCallback, this, std::placeholders::_1), map_update_options);
 
   map_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the semantic map subscriber