From 9ca1caa38f772be9b3d9555497f7152a0d77f645 Mon Sep 17 00:00:00 2001 From: Anish Date: Fri, 19 Apr 2024 09:31:44 -0400 Subject: [PATCH 1/4] add spat wall time process --- carma/launch/carma_src.launch.py | 8 ++++++++ carma_wm/include/carma_wm/CARMAWorldModel.hpp | 6 ++++-- carma_wm/src/CARMAWorldModel.cpp | 19 +++++++++++++------ carma_wm/src/WMListener.cpp | 10 +++++++++- carma_wm/src/WMListenerWorker.cpp | 6 +++++- carma_wm/src/WMListenerWorker.hpp | 6 ++++++ 6 files changed, 45 insertions(+), 10 deletions(-) diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py index a0f9e87998..2413e2b314 100644 --- a/carma/launch/carma_src.launch.py +++ b/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( @@ -330,6 +337,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, diff --git a/carma_wm/include/carma_wm/CARMAWorldModel.hpp b/carma_wm/include/carma_wm/CARMAWorldModel.hpp index 36c0306c7a..483d66b4e9 100644 --- a/carma_wm/include/carma_wm/CARMAWorldModel.hpp +++ b/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 diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 5b06475573..de168c8bed 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -1427,10 +1427,10 @@ 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 @@ -1438,8 +1438,15 @@ namespace carma_wm { 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::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(current_movement_event.event_state.movement_phase_state); diff --git a/carma_wm/src/WMListener.cpp b/carma_wm/src/WMListener.cpp index 2a3bb2cad9..87883ff94e 100644 --- a/carma_wm/src/WMListener.cpp +++ b/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 system 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; diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index 1a1325ba57..675368e106 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/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 { diff --git a/carma_wm/src/WMListenerWorker.hpp b/carma_wm/src/WMListenerWorker.hpp index 4f4aedf6a3..568d1ad665 100644 --- a/carma_wm/src/WMListenerWorker.hpp +++ b/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 world_model_; bool use_sim_time_; + bool is_spat_wall_time_; std::function map_callback_; std::function route_callback_; void newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement* regem) const; From b8518c92100c97d1993fa771e89cf9fd85092bac Mon Sep 17 00:00:00 2001 From: Anish Date: Fri, 19 Apr 2024 09:52:35 -0400 Subject: [PATCH 2/4] add param to launch files --- carma/launch/carma_src.launch.py | 3 ++- carma/launch/guidance.launch.py | 11 ++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py index 2413e2b314..ab7866ffa8 100644 --- a/carma/launch/carma_src.launch.py +++ b/carma/launch/carma_src.launch.py @@ -287,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() ), ] diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index ca32cc646c..4db8504bdb 100644 --- a/carma/launch/guidance.launch.py +++ b/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, From 6638f80ec75c6424403adc3c6e18cb2326e2fece Mon Sep 17 00:00:00 2001 From: Anish Date: Fri, 19 Apr 2024 13:16:51 -0400 Subject: [PATCH 3/4] update comment --- carma_wm/src/WMListener.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/carma_wm/src/WMListener.cpp b/carma_wm/src/WMListener.cpp index 87883ff94e..beef1c9596 100644 --- a/carma_wm/src/WMListener.cpp +++ b/carma_wm/src/WMListener.cpp @@ -68,7 +68,7 @@ WMListener::WMListener( 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 system time? : " << is_spat_wall_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()); From 24c16d4329650234d84dfb2da75e7e66e9a8eb51 Mon Sep 17 00:00:00 2001 From: Anish Deva Date: Fri, 19 Apr 2024 18:42:47 -0400 Subject: [PATCH 4/4] cast chrono time --- carma_wm/src/CARMAWorldModel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index de168c8bed..003b519791 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -1442,7 +1442,7 @@ namespace carma_wm { // 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::system_clock::now().time_since_epoch()).count() - ros1_clock_.value().seconds(); + wall_time_offset_in_seconds = (std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count()) - ros1_clock_.value().seconds(); } min_end_time += lanelet::time::durationFromSec(simulation_time_difference_in_seconds);