add spat wall time process (#2366)

carma/launch/carma_src.launch.py

@@ -107,6 +107,13 @@ def generate_launch_description():
  description = "True of simulation mode is on"
  )
 
+ is_spat_wall_time = LaunchConfiguration('is_spat_wall_time')
+ declare_is_spat_wall_time_arg = DeclareLaunchArgument(
+ name = 'is_spat_wall_time',
+ default_value = 'True',
+ description = "True if SPaT is being published based on wall clock"
+ )
+
  #Declare the route file folder launch argument
  route_file_folder = LaunchConfiguration('route_file_folder')
  declare_route_file_folder = DeclareLaunchArgument(
@@ -280,7 +287,8 @@ def generate_launch_description():
  'tactical_plugins_to_validate' : tactical_plugins_to_validate,
  'control_plugins_to_validate' : control_plugins_to_validate,
  'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
- 'use_sim_time' : use_sim_time
+ 'use_sim_time' : use_sim_time,
+ 'is_spat_wall_time' : is_spat_wall_time
  }.items()
  ),
  ]
@@ -330,6 +338,7 @@ def generate_launch_description():
  declare_vehicle_characteristics_param_file_arg,
  declare_vehicle_config_param_file_arg,
  declare_use_sim_time_arg,
+ declare_is_spat_wall_time_arg,
  declare_route_file_folder,
  declare_enable_guidance_plugin_validator,
  declare_strategic_plugins_to_validate,

carma/launch/guidance.launch.py

@@ -60,6 +60,13 @@ def generate_launch_description():
  description = "True if simulation mode is on"
  )
 
+ is_spat_wall_time = LaunchConfiguration('is_spat_wall_time')
+ declare_is_spat_wall_time_arg = DeclareLaunchArgument(
+ name = 'is_spat_wall_time',
+ default_value = 'True',
+ description = "True if SPaT is being published based on wall clock"
+ )
+
  subsystem_controller_default_param_file = os.path.join(
  get_package_share_directory('subsystem_controllers'), 'config/guidance_controller_config.yaml')
 
@@ -393,7 +400,8 @@ def generate_launch_description():
  parameters=[
  subsystem_controller_default_param_file,
  subsystem_controller_param_file,
- {"use_sim_time" : use_sim_time}],
+ {"use_sim_time" : use_sim_time},
+ {"is_spat_wall_time" : is_spat_wall_time}],
  on_exit= Shutdown(), # Mark the subsystem controller as required
  arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
  )
@@ -402,6 +410,7 @@ def generate_launch_description():
  declare_vehicle_config_param_file_arg,
  declare_use_sim_time_arg,
  declare_subsystem_controller_param_file_arg,
+ declare_is_spat_wall_time_arg,
  carma_trajectory_executor_and_route_container,
  carma_guidance_visualizer_container,
  carma_guidance_worker_container,

carma_wm/include/carma_wm/CARMAWorldModel.hpp

@@ -111,8 +111,9 @@ class CARMAWorldModel : public WorldModel
  *
  * @param spat_msg Msg to update with
  * @param use_sim_time Boolean to indicate if it is currently simulation or not
+ * @param is_spat_wall_time Boolean to indicate if the incoming spat is based on wall clock. Defaults to true.
  */
- void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false);
+ void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false, bool is_spat_wall_time=true);
 
  /**
  * \brief This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane.
@@ -135,8 +136,9 @@ class CARMAWorldModel : public WorldModel
  * \param min_end_time minimum end time of the spat movement event list
  * \param moy_exists tells weather minute of the year exist or not
  * \param moy value of the minute of the year
+ * \param is_spat_wall_time Boolean to indicate if the incoming spat is based on wall clock. Defaults to true.
  */
- 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);
+ 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 = true, bool is_spat_wall_time=false);
 
 /** \param config_lim the configurable speed limit value populated from WMListener using the config_speed_limit parameter
  * in VehicleConfigParams.yaml

carma_wm/src/CARMAWorldModel.cpp

@@ -1427,19 +1427,26 @@ namespace carma_wm
  return curr_light;
  }
 
- 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)
+ 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, bool is_spat_wall_time)
  {
  double simulation_time_difference_in_seconds = 0.0;
-
+ double wall_time_offset_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_ && simulation_clock_)
  {
  simulation_time_difference_in_seconds = ros1_clock_.value().seconds() - simulation_clock_.value().seconds();
  }
+ else if (is_simulation && ros1_clock_ && is_spat_wall_time)
+ {
+ // NOTE: If carma-platform is running in simulation with clock synced to sim time but the incoming spat information is based on wall clock
+ // the spat signal phase time must be offset to sim time.
+ wall_time_offset_in_seconds = (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count()) - ros1_clock_.value().seconds();
+ }
 
  min_end_time += lanelet::time::durationFromSec(simulation_time_difference_in_seconds);
+ min_end_time -= lanelet::time::durationFromSec(wall_time_offset_in_seconds);
 
  if (moy_exists) //account for minute of the year
  {
@@ -1450,7 +1457,7 @@ namespace carma_wm
  int curr_year = curr_time_boost.date().year();
 
  // Force the current year to start of epoch if it is simulation
- if (is_simulation)
+ if (is_simulation && !is_spat_wall_time)
  curr_year = 1970;
 
  auto curr_year_start_boost(boost::posix_time::time_from_string(std::to_string(curr_year)+ "-01-01 00:00:00.000"));
@@ -1473,7 +1480,7 @@ namespace carma_wm
  }
  }
 
- void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg, bool use_sim_time)
+ void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg, bool use_sim_time, bool is_spat_wall_time)
  {
  if (!semantic_map_)
  {
@@ -1536,8 +1543,8 @@ namespace carma_wm
  boost::posix_time::ptime min_end_time_dynamic = lanelet::time::timeFromSec(current_movement_event.timing.min_end_time);
  boost::posix_time::ptime start_time_dynamic = lanelet::time::timeFromSec(current_movement_event.timing.start_time);
 
- min_end_time_dynamic=min_end_time_converter_minute_of_year(min_end_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year
- start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year
+ min_end_time_dynamic=min_end_time_converter_minute_of_year(min_end_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time, is_spat_wall_time); // Accounting minute of the year
+ start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time, is_spat_wall_time); // Accounting minute of the year
 
  auto received_state_dynamic = static_cast<lanelet::CarmaTrafficSignalState>(current_movement_event.event_state.movement_phase_state);
 

carma_wm/src/WMListener.cpp

@@ -53,20 +53,28 @@ WMListener::WMListener(
  use_sim_time_param_value = node_params_->declare_parameter("use_sim_time", rclcpp::ParameterValue (false));
  }
 
+ rclcpp::Parameter is_spat_wall_time_param("is_spat_wall_time");
+ if(!node_params_->get_parameter("is_spat_wall_time", is_spat_wall_time_param)){
+ rclcpp::ParameterValue is_spat_wall_time_param_value;
+ is_spat_wall_time_param_value = node_params_->declare_parameter("is_spat_wall_time", rclcpp::ParameterValue (true));
+ }
+
  // 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");
-
+ is_spat_wall_time_param = node_params_->get_parameter("is_spat_wall_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());
  RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is using simulation time? : " << use_sim_time_param.as_bool());
+ RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is SPaT using wall time? : " << is_spat_wall_time_param.as_bool());
 
 
  setConfigSpeedLimit(config_speed_limit_param.as_double());
  worker_->setVehicleParticipationType(participant_param.as_string());
  worker_->isUsingSimTime(use_sim_time_param.as_bool());
+ worker_->isSpatWallTime(is_spat_wall_time_param.as_bool());
 
  rclcpp::SubscriptionOptions map_update_options;
  rclcpp::SubscriptionOptions map_options;

carma_wm/src/WMListenerWorker.cpp

@@ -103,7 +103,7 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::Sh
 
 void WMListenerWorker::incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::SharedPtr spat_msg)
 {
- world_model_->processSpatFromMsg(*spat_msg, use_sim_time_);
+ world_model_->processSpatFromMsg(*spat_msg, use_sim_time_, is_spat_wall_time_);
 }
 
 bool WMListenerWorker::checkIfReRoutingNeeded() const
@@ -705,6 +705,10 @@ void WMListenerWorker::isUsingSimTime(bool use_sim_time)
 {
  use_sim_time_ = use_sim_time;
 }
+void WMListenerWorker::isSpatWallTime(bool is_spat_wall_time)
+{
+ is_spat_wall_time_ = is_spat_wall_time;
+}
 
 double WMListenerWorker::getConfigSpeedLimit() const
 {

carma_wm/src/WMListenerWorker.hpp

@@ -151,9 +151,15 @@ class WMListenerWorker
  */
  void isUsingSimTime(bool use_sim_time);
 
+ /**
+ * \brief set true if incoming spat is based on wall clock
+ */
+ void isSpatWallTime(bool is_spat_wall_time);
+
 private:
  std::shared_ptr<CARMAWorldModel> world_model_;
  bool use_sim_time_;
+ bool is_spat_wall_time_;
  std::function<void()> map_callback_;
  std::function<void()> route_callback_;
  void newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement* regem) const;