diff --git a/.circleci/config.yml b/.circleci/config.yml index adc9c72cca..944d1d59f0 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -62,12 +62,14 @@ jobs: command: | source ${INIT_ENV} export ROS_PARALLEL_JOBS='-j4 -l4' # Try to reduce memory consumption on build + sed -i 's/colcon build /colcon build --packages-skip novatel_oem7_msgs tracetools tracetools_test /' /home/carma/.ci-image/engineering_tools/code_coverage/make_with_coverage.bash make_with_coverage.bash -m -e /opt/carma/ -o ./coverage_reports/gcov - run: name: Run C++ Tests ROS 1 command: | source ${INIT_ENV} export ROS_PARALLEL_JOBS='-j4 -l4' # Try to reduce memory consumption on build + sed -i 's/colcon test /colcon test --packages-skip novatel_oem7_msgs tracetools tracetools_test /' /home/carma/.ci-image/engineering_tools/code_coverage/make_with_coverage.bash make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov - run: name: Backup ROS1 compile_commands.json diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 087edbcab2..3606fde978 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -1,18 +1,18 @@ # Copyright (C) 2018-2021 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. -# Configuration file for Sonar Scanner used for CI +# Configuration file for Sonar Scanner used for CI sonar.scanner.force-deprecated-java-version-grace-period=true sonar.projectKey=usdot-fhwa-stol_CARMAPlatform sonar.organization=usdot-fhwa-stol @@ -41,7 +41,6 @@ sonar.modules= bsm_generator, \ route, \ route_following_plugin, \ trajectory_executor, \ - health_monitor, \ localization_manager, \ arbitrator, \ plan_delegator, \ @@ -78,7 +77,8 @@ sonar.modules= bsm_generator, \ platoon_control_ihp, \ carma_guidance_plugins, \ carma_cloud_client, \ - approaching_emergency_vehicle_plugin + approaching_emergency_vehicle_plugin, \ + carma_cooperative_perception guidance.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/guidance bsm_generator.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/bsm_generator @@ -88,7 +88,6 @@ route.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/ro route_following_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/route_following_plugin trajectory_executor.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/trajectory_executor localization_manager.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/localization_manager -health_monitor.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/health_monitor arbitrator.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/arbitrator plan_delegator.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/plan_delegator carma_wm.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_wm @@ -126,6 +125,7 @@ lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/lci_str carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_guidance_plugins carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_cloud_client approaching_emergency_vehicle_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/approaching_emergency_vehicle_plugin +carma_cooperative_perception.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_cooperative_perception # C++ Package differences # Sources @@ -145,8 +145,6 @@ trajectory_executor.sonar.sources = src trajectory_executor.sonar.exclusions =test/** localization_manager.sonar.sources = src localization_manager.sonar.exclusions =test/** -health_monitor.sonar.sources = src -health_monitor.sonar.exclusions =test/** arbitrator.sonar.sources = src arbitrator.sonar.exclusions =test/** plan_delegator.sonar.sources = src @@ -221,6 +219,8 @@ carma_cloud_client.sonar.sources = src carma_cloud_client.sonar.exclusions =test/** approaching_emergency_vehicle_plugin.sonar.sources = src approaching_emergency_vehicle_plugin.sonar.exclusions =test/** +carma_cooperative_perception.sonar.sources = src +carma_cooperative_perception.sonar.exclusions = test/** # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. @@ -231,17 +231,16 @@ pure_pursuit_wrapper.sonar.tests = test route_generator.sonar.tests = test trajectory_executor.sonar.tests = test localization_manager.sonar.tests = test -health_monitor.sonar.tests = test arbitrator.sonar.tests = test carma_wm.sonar.tests = test carma_wm_ctrl.sonar.tests = test mpc_follower_wrapper.sonar.tests = test truck_inspection_client.sonar.tests = test -roadway_objects.sonar.tests = test +roadway_objects.sonar.tests = test platooning_strategic_IHP.sonar.tests = test -platooning_tactical_plugin.sonar.tests = test +platooning_tactical_plugin.sonar.tests = test platoon_control_plugin.sonar.tests = test -mobilitypath_publisher.sonar.tests = test +mobilitypath_publisher.sonar.tests = test mock_lightbar_driver.sonar.tests = test port_drayage_plugin.sonar.tests = test rosbag_mock_drivers.sonar.tests = test @@ -271,3 +270,4 @@ lci_strategic_plugin.sonar.sources = test carma_guidance_plugins.sonar.sources = test carma_cloud_client.sonar.sources = test approaching_emergency_vehicle_plugin.sonar.sources = test +carma_cooperative_perception.tests = test diff --git a/arbitrator/include/arbitrator.hpp b/arbitrator/include/arbitrator.hpp index 5940bd2e1f..74a1cbf51c 100644 --- a/arbitrator/include/arbitrator.hpp +++ b/arbitrator/include/arbitrator.hpp @@ -32,11 +32,11 @@ #include "planning_strategy.hpp" #include "capabilities_interface.hpp" -namespace arbitrator +namespace arbitrator { /** * Primary work class for the Arbitrator package - * + * * Governs the interactions of plugins during the maneuver planning phase of * the CARMA planning process. Utilizes a generic planning interface to allow * for reconfiguration with other paradigms in the future. @@ -53,27 +53,27 @@ namespace arbitrator * \param min_plan_duration The minimum acceptable length of a plan * \param planning_frequency The frequency at which to generate high-level plans when engaged * \param wm pointer to an inialized world model. - */ + */ Arbitrator(std::shared_ptr nh, - std::shared_ptr sm, - std::shared_ptr ci, + std::shared_ptr sm, + std::shared_ptr ci, std::shared_ptr planning_strategy, rclcpp::Duration min_plan_duration, double planning_period, - carma_wm::WorldModelConstPtr wm): + carma_wm::WorldModelConstPtr wm): sm_(sm), nh_(nh), + min_plan_duration_(min_plan_duration), + time_between_plans_(planning_period), capabilities_interface_(ci), planning_strategy_(planning_strategy), initialized_(false), - min_plan_duration_(min_plan_duration), - time_between_plans_(planning_period), wm_(wm), tf2_buffer_(nh_->get_clock()) {}; - + /** * \brief Begin the operation of the arbitrator. - * + * * Loops internally via rclcpp::Duration sleeps and spins */ void run(); @@ -93,7 +93,7 @@ namespace arbitrator * \brief Initialize transform Lookup from front bumper to map */ void initializeBumperTransformLookup(); - + protected: /** * \brief Function to be executed during the initial state of the Arbitrator @@ -129,9 +129,9 @@ namespace arbitrator void guidance_state_cb(carma_planning_msgs::msg::GuidanceState::UniquePtr msg); private: - + VehicleState vehicle_state_; // The current state of the vehicle for populating planning requests - + std::shared_ptr sm_; carma_ros2_utils::PubPtr final_plan_pub_; carma_ros2_utils::SubPtr guidance_state_sub_; @@ -152,6 +152,6 @@ namespace arbitrator tf2::Stamped bumper_transform_; bool planning_in_progress_ = false; }; -}; +} #endif diff --git a/arbitrator/include/arbitrator_config.hpp b/arbitrator/include/arbitrator_config.hpp index 9562104a9d..6f3dcafea2 100644 --- a/arbitrator/include/arbitrator_config.hpp +++ b/arbitrator/include/arbitrator_config.hpp @@ -25,7 +25,7 @@ #include -namespace arbitrator +namespace arbitrator { // Stream operator for map data structure std::ostream &operator<<(std::ostream &output, const std::map &map) @@ -37,25 +37,25 @@ namespace arbitrator } output << "}"; return output; - }; + } /** * \brief Config struct */ struct Config { - double min_plan_duration = 6.0; // The minimum amount of time in seconds that an arbitrated plan must cover for the + double min_plan_duration = 6.0; // The minimum amount of time in seconds that an arbitrated plan must cover for the // system to proceed with execution - double target_plan_duration = 15.0; // The nominal amount of time in seconds that an arbitrated plan should cover for the + double target_plan_duration = 15.0; // The nominal amount of time in seconds that an arbitrated plan should cover for the // system to operate at best performance double planning_frequency = 1.0; // The planning frequency (hz) for generation for arbitrated plans - int beam_width = 3; // The width of the search beam to use for arbitrator planning, 1 = + int beam_width = 3; // The width of the search beam to use for arbitrator planning, 1 = // greedy search, as it approaches infinity the search approaches breadth-first search - bool use_fixed_costs = false; // Use fixed priority cost function over using the cost system for + bool use_fixed_costs = false; // Use fixed priority cost function over using the cost system for // evaluating maneuver plans - std::map plugin_priorities = {}; // The priorities associated with each plugin during the planning + std::map plugin_priorities = {}; // The priorities associated with each plugin during the planning // process, values will be normalized at runtime and inverted into costs - + // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const Config &c) { @@ -73,4 +73,4 @@ namespace arbitrator } // namespace abitrator -#endif \ No newline at end of file +#endif diff --git a/arbitrator/include/capabilities_interface.hpp b/arbitrator/include/capabilities_interface.hpp index 9d1dc390ce..43dd97cb52 100644 --- a/arbitrator/include/capabilities_interface.hpp +++ b/arbitrator/include/capabilities_interface.hpp @@ -53,7 +53,7 @@ namespace arbitrator /** * \brief Get the list of topics that respond to the capability specified by * the query string - * + * * \param query_string The string name of the capability to look for * \return A list of all responding topics, if any are found. */ @@ -62,19 +62,19 @@ namespace arbitrator /** * \brief Template function for calling all nodes which respond to a service associated - * with a particular capability. Will send the service request to all nodes and + * with a particular capability. Will send the service request to all nodes and * aggregate the responses. - * + * * \tparam MSrvReq The typename of the service message request * \tparam MSrvRes The typename of the service message response - * + * * \param query_string The string name of the capability to look for * \param The message itself to send * \return A map matching the topic name that responded -> the response */ template std::map> multiplex_service_call_for_capability(const std::string& query_string, std::shared_ptr msg); - + const static std::string STRATEGIC_PLAN_CAPABILITY; protected: @@ -82,13 +82,13 @@ namespace arbitrator std::shared_ptr nh_; carma_ros2_utils::ClientPtr sc_s_; - std::unordered_set capabilities_ ; + std::unordered_set capabilities_ ; + - }; -}; +} #include "capabilities_interface.tpp" diff --git a/arbitrator/include/capabilities_interface.tpp b/arbitrator/include/capabilities_interface.tpp index ef175fb772..bc74d9ac71 100644 --- a/arbitrator/include/capabilities_interface.tpp +++ b/arbitrator/include/capabilities_interface.tpp @@ -22,37 +22,43 @@ #include #include #include +#include -namespace arbitrator +namespace arbitrator { template - std::map> + std::map> CapabilitiesInterface::multiplex_service_call_for_capability(const std::string& query_string, std::shared_ptr msg) { - std::vector topics = get_topics_for_capability(query_string); - std::map> responses; - for (auto i = topics.begin(); i != topics.end(); i++) + for (const auto & topic : get_topics_for_capability(query_string)) { - auto topic = *i; - auto sc = nh_->create_client(topic); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << topic); - - std::shared_future> resp = sc->async_send_request(msg); - - auto future_status = resp.wait_for(std::chrono::milliseconds(500)); - - if (future_status == std::future_status::ready) { - responses.emplace(topic, resp.get()); - } - else - { - RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "failed...: " << topic); + try { + using std::literals::chrono_literals::operator""ms; + + const auto client = nh_->create_client(topic); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << topic); + + const auto response = client->async_send_request(msg); + + switch (const auto status{response.wait_for(500ms)}; status) { + case std::future_status::ready: + responses.emplace(topic, response.get()); + break; + case std::future_status::deferred: + case std::future_status::timeout: + RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "failed...: " << topic); + } + } catch(const rclcpp::exceptions::RCLError& error) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("arbitrator"), + "Cannot make service request for service '" << topic << "': " << error.what()); + continue; } } + return responses; } -}; +} #endif diff --git a/arbitrator/include/cost_function.hpp b/arbitrator/include/cost_function.hpp index 1cc9125a37..cb5df72a9b 100644 --- a/arbitrator/include/cost_function.hpp +++ b/arbitrator/include/cost_function.hpp @@ -48,6 +48,6 @@ namespace arbitrator */ virtual ~CostFunction(){}; }; -}; +} -#endif //__ARBITRATOR_INCLUDE_COST_FUNCTION_HPP__ \ No newline at end of file +#endif //__ARBITRATOR_INCLUDE_COST_FUNCTION_HPP__ diff --git a/arbitrator/include/cost_system_cost_function.hpp b/arbitrator/include/cost_system_cost_function.hpp index affb888cdf..c38a9fce04 100644 --- a/arbitrator/include/cost_system_cost_function.hpp +++ b/arbitrator/include/cost_system_cost_function.hpp @@ -28,9 +28,9 @@ namespace arbitrator { /** * \brief Implementation of the CostFunction interface - * + * * Implements costs by utilizing a ROS service call to the CARMA cost plugin - * system. Passes on the input maneuver plan to the Cost Plugin System for + * system. Passes on the input maneuver plan to the Cost Plugin System for * computation of the actual cost and cost per unit distance. */ class CostSystemCostFunction : public CostFunction @@ -44,9 +44,9 @@ namespace arbitrator /** * Initialize the CostSystemCostFunction to communicate over the network. * Sets up any ROS service clients needed to interact with the cost plugin system. - * + * * Must be called before using this cost function implementation. - * + * * \param nh A publicly namespaced nodehandle */ void init(std::shared_ptr nh); @@ -55,7 +55,7 @@ namespace arbitrator * \brief Compute the unit cost over distance of a given maneuver plan * \param plan The plan to evaluate * \return double The total cost divided by the total distance of the plan - * + * * \throws std::logic_error if not initialized */ double compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan); @@ -68,10 +68,9 @@ namespace arbitrator */ double compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan); private: - carma_ros2_utils::ClientPtr cost_system_sc_; + carma_ros2_utils::ClientPtr cost_system_sc_; bool initialized_ = false; }; -}; +} #endif //__ARBITRATOR_INCLUDE_COST_SYSTEM_COST_FUNCTION_HPP__ - diff --git a/arbitrator/include/fixed_priority_cost_function.hpp b/arbitrator/include/fixed_priority_cost_function.hpp index 0e27d7d65a..8414600fb3 100644 --- a/arbitrator/include/fixed_priority_cost_function.hpp +++ b/arbitrator/include/fixed_priority_cost_function.hpp @@ -25,12 +25,12 @@ namespace arbitrator { /** * \brief Implementation of the CostFunction interface - * + * * Implements costs by associating a fixed priority number with each plugin * (as specified by configuration). This priority is then normalized across - * all plugins, and then an inverse is computed to arrive at the cost per + * all plugins, and then an inverse is computed to arrive at the cost per * unit distance for that plugins. - * + * * e.g. Three plugins with priority 20, 10, and 5 will respectively have * costs 0, 0.5, 0.75 per unit distance. */ @@ -59,6 +59,6 @@ namespace arbitrator private: std::map plugin_costs_; }; -}; +} #endif //__ARBITRATOR_INCLUDE_FIXED_PRIORITY_COST_FUNCTION_HPP__ diff --git a/arbitrator/include/planning_strategy.hpp b/arbitrator/include/planning_strategy.hpp index c5f1dea529..12a77970cd 100644 --- a/arbitrator/include/planning_strategy.hpp +++ b/arbitrator/include/planning_strategy.hpp @@ -32,9 +32,9 @@ namespace arbitrator public: /** * \brief Generate a plausible maneuver plan - * + * * \param start_state The starting state of the vehicle to plan for - * + * * \return A maneuver plan from the vehicle's current state */ virtual carma_planning_msgs::msg::ManeuverPlan generate_plan(const VehicleState& start_state) = 0; @@ -44,6 +44,6 @@ namespace arbitrator */ virtual ~PlanningStrategy(){}; }; -}; +} -#endif //__ARBITRATOR_INCLUDE_PLANNING_STRATEGY_HPP__ \ No newline at end of file +#endif //__ARBITRATOR_INCLUDE_PLANNING_STRATEGY_HPP__ diff --git a/arbitrator/include/plugin_neighbor_generator.hpp b/arbitrator/include/plugin_neighbor_generator.hpp index 3e490ffe27..f17c8103bf 100644 --- a/arbitrator/include/plugin_neighbor_generator.hpp +++ b/arbitrator/include/plugin_neighbor_generator.hpp @@ -25,11 +25,11 @@ namespace arbitrator { /** * \brief Implementation of the NeighborGenerator interface using plugins - * + * * Queries plugins via the CapabilitiesInterface to contribute additional * maneuvers as the potential child/neighbor nodes of a plan in progress. - * - * \tparam T The type of CapabilitiesInterface to use. Templated to enable + * + * \tparam T The type of CapabilitiesInterface to use. Templated to enable * testing */ template @@ -44,7 +44,7 @@ namespace arbitrator ci_(ci) {}; /** - * Generates a list of neighbor states for the given plan using + * Generates a list of neighbor states for the given plan using * the plugins available to the system * \param plan The plan that is the current search state * \param initial_state The initial state of the vehicle at the start of plan. This will be provided to planners for specific use when plan is empty @@ -54,8 +54,8 @@ namespace arbitrator private: std::shared_ptr ci_; }; -}; +} #include "plugin_neighbor_generator.tpp" -#endif //__ARBITRATOR_INCLUDE_PLUGIN_NEIGHBOR_GENERATOR_HPP__ \ No newline at end of file +#endif //__ARBITRATOR_INCLUDE_PLUGIN_NEIGHBOR_GENERATOR_HPP__ diff --git a/arbitrator/include/tree_planner.hpp b/arbitrator/include/tree_planner.hpp index 80759b893c..c75b103e1c 100644 --- a/arbitrator/include/tree_planner.hpp +++ b/arbitrator/include/tree_planner.hpp @@ -29,12 +29,12 @@ namespace arbitrator { /** - * \brief Implementation of PlanningStrategy using a generic tree search + * \brief Implementation of PlanningStrategy using a generic tree search * algorithm - * + * * The fundamental components of this tree search are individually injected * into this class at construction time to allow for fine-tuning of the - * algorithm and ensure better testability and separation of algorithmic + * algorithm and ensure better testability and separation of algorithmic * concerns */ class TreePlanner : public PlanningStrategy @@ -47,9 +47,9 @@ namespace arbitrator * \param ss Shared ptr to a SearchStrategy implementation * \param target The desired duration of finished plans */ - TreePlanner(std::shared_ptr cf, - std::shared_ptr ng, - std::shared_ptr ss, + TreePlanner(std::shared_ptr cf, + std::shared_ptr ng, + std::shared_ptr ss, rclcpp::Duration target): cost_function_(cf), neighbor_generator_(ng), @@ -57,9 +57,9 @@ namespace arbitrator target_plan_duration_(target) {}; /** - * \brief Utilize the configured cost function, neighbor generator, + * \brief Utilize the configured cost function, neighbor generator, * and search strategy, to generate a plan by means of tree search - * + * * \param start_state The starting state of the vehicle to plan for */ carma_planning_msgs::msg::ManeuverPlan generate_plan(const VehicleState& start_state); @@ -69,6 +69,6 @@ namespace arbitrator std::shared_ptr search_strategy_; rclcpp::Duration target_plan_duration_; }; -}; +} -#endif //__ARBITRATOR_INCLUDE_TREE_PLANNER_HPP__ \ No newline at end of file +#endif //__ARBITRATOR_INCLUDE_TREE_PLANNER_HPP__ diff --git a/arbitrator/src/arbitrator.cpp b/arbitrator/src/arbitrator.cpp index 3150519346..d3d99d3372 100644 --- a/arbitrator/src/arbitrator.cpp +++ b/arbitrator/src/arbitrator.cpp @@ -29,8 +29,8 @@ namespace arbitrator if (!planning_in_progress_ && nh_->get_current_state().id() != lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN) { planning_in_progress_ = true; - - switch (sm_->get_state()) + + switch (sm_->get_state()) { case INITIAL: RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in INITIAL state."); @@ -59,12 +59,12 @@ namespace arbitrator } else { - return; + return; } planning_in_progress_ = false; } - - void Arbitrator::guidance_state_cb(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) + + void Arbitrator::guidance_state_cb(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) { switch (msg->state) { @@ -105,7 +105,7 @@ namespace arbitrator void Arbitrator::initial_state() { if(!initialized_) - { + { RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator initializing on first initial state spin..."); final_plan_pub_ = nh_->create_publisher("final_maneuver_plan", 5); guidance_state_sub_ = nh_->create_subscription("guidance_state", 5, std::bind(&Arbitrator::guidance_state_cb, this, std::placeholders::_1)); @@ -121,20 +121,20 @@ namespace arbitrator { RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator beginning planning process!"); rclcpp::Time planning_process_start = nh_->get_clock()->now(); - + carma_planning_msgs::msg::ManeuverPlan plan = planning_strategy_->generate_plan(vehicle_state_); - if (!plan.maneuvers.empty()) + if (!plan.maneuvers.empty()) { rclcpp::Time plan_end_time = arbitrator_utils::get_plan_end_time(plan); rclcpp::Time plan_start_time = arbitrator_utils::get_plan_start_time(plan); rclcpp::Duration plan_duration = plan_end_time - plan_start_time; - if (plan_duration < min_plan_duration_) + if (plan_duration < min_plan_duration_) { RCLCPP_WARN_STREAM(nh_->get_logger(), "Arbitrator is unable to generate a plan with minimum plan duration requirement!"); - } - else + } + else { RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator is publishing plan " << std::string(plan.maneuver_plan_id) << " of duration " << plan_duration.seconds() << " as current maneuver plan"); } @@ -203,14 +203,14 @@ namespace arbitrator } - void Arbitrator::twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg) + void Arbitrator::twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg) { vehicle_state_.velocity = msg->twist.linear.x; } - void Arbitrator::initializeBumperTransformLookup() + void Arbitrator::initializeBumperTransformLookup() { tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_)); tf2_buffer_.setUsingDedicatedThread(true); } -}; +} diff --git a/arbitrator/src/arbitrator_node.cpp b/arbitrator/src/arbitrator_node.cpp index 4dc9c7d30b..7b9894cfc0 100644 --- a/arbitrator/src/arbitrator_node.cpp +++ b/arbitrator/src/arbitrator_node.cpp @@ -19,7 +19,7 @@ namespace arbitrator { - ArbitratorNode::ArbitratorNode(const rclcpp::NodeOptions& options) : carma_ros2_utils::CarmaLifecycleNode(options) + ArbitratorNode::ArbitratorNode(const rclcpp::NodeOptions& options) : carma_ros2_utils::CarmaLifecycleNode(options) { // Create initial config config_ = Config(); @@ -31,16 +31,16 @@ namespace arbitrator config_.use_fixed_costs = declare_parameter("use_fixed_costs", config_.use_fixed_costs); config_.plugin_priorities = plugin_priorities_map_from_json(declare_parameter("plugin_priorities", "")); } - + carma_ros2_utils::CallbackReturn ArbitratorNode::handle_on_configure(const rclcpp_lifecycle::State &) { // Handle dependency injection auto ci = std::make_shared(shared_from_this()); arbitrator::ArbitratorStateMachine sm; - + // Reset config config_ = Config(); - + get_parameter("min_plan_duration", config_.min_plan_duration); get_parameter("target_plan_duration", config_.target_plan_duration); get_parameter("planning_frequency", config_.planning_frequency); @@ -68,7 +68,7 @@ namespace arbitrator auto png = std::make_shared>(ci); arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration(config_.target_plan_duration* 1e9)); - + wm_listener_ = std::make_shared( this->get_node_base_interface(), this->get_node_logging_interface(), this->get_node_topics_interface(), this->get_node_parameters_interface() @@ -78,32 +78,32 @@ namespace arbitrator arbitrator_ = std::make_shared( shared_from_this(), - std::make_shared(sm), - ci, - std::make_shared(tp), + std::make_shared(sm), + ci, + std::make_shared(tp), rclcpp::Duration(config_.min_plan_duration* 1e9), 1/config_.planning_frequency, wm_ ); - - + + twist_sub_ = create_subscription("current_velocity", 1, std::bind(&Arbitrator::twist_cb, arbitrator_.get(), std::placeholders::_1)); arbitrator_->initializeBumperTransformLookup(); return CallbackReturn::SUCCESS; } - + carma_ros2_utils::CallbackReturn ArbitratorNode::handle_on_activate(const rclcpp_lifecycle::State &) { bumper_pose_timer_ = create_timer(get_clock(), std::chrono::milliseconds(100), [this]() {this->arbitrator_->bumper_pose_cb();}); - + arbitrator_run_ = create_timer(get_clock(), std::chrono::duration(1/(config_.planning_frequency * 2 )), //there is waiting state between each planning state [this]() {this->arbitrator_->run();}); //The intention is to keep the arbitrator planning at intended frequency - 1s. Looks like ROS2 conversion kept the WAITING state from ROS1, - //which occurs between PLANNING state in the state machine. So if the timer calls state callback each 1 second, the arbitrator ends up planning + //which occurs between PLANNING state in the state machine. So if the timer calls state callback each 1 second, the arbitrator ends up planning //every 2s due to PLANNING > WAITING > PLANNING transition. That's why the frequency is doubled. RCLCPP_INFO_STREAM(rclcpp::get_logger("arbitrator"), "Arbitrator started, beginning arbitrator state machine."); return CallbackReturn::SUCCESS; @@ -112,7 +112,7 @@ namespace arbitrator std::map ArbitratorNode::plugin_priorities_map_from_json(const std::string& json_string) { std::map map; - + rapidjson::Document d; if(d.Parse(json_string.c_str()).HasParseError()) { @@ -125,16 +125,16 @@ namespace arbitrator } rapidjson::Value& map_value = d["plugin_priorities"]; - for (rapidjson::Value::ConstMemberIterator it = map_value.MemberBegin(); - it != map_value.MemberEnd(); it++) + for (rapidjson::Value::ConstMemberIterator it = map_value.MemberBegin(); + it != map_value.MemberEnd(); it++) { - map[it->name.GetString()] = it->value.GetDouble(); + map[it->name.GetString()] = it->value.GetDouble(); } return map; } -}; +} #include "rclcpp_components/register_node_macro.hpp" diff --git a/carma/launch/carma_src.launch b/carma/launch/carma_src.launch index 1ebcb2b1b2..c6af598674 100755 --- a/carma/launch/carma_src.launch +++ b/carma/launch/carma_src.launch @@ -1,6 +1,6 @@ - @@ -99,37 +98,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - + - + diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py index 6165f29d05..a448707ae8 100644 --- a/carma/launch/carma_src.launch.py +++ b/carma/launch/carma_src.launch.py @@ -1,4 +1,4 @@ -# Copyright (C) 2021-2022 LEIDOS. +# Copyright (C) 2021-2023 LEIDOS. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,6 +13,7 @@ # limitations under the License. from ament_index_python import get_package_share_directory +import launch from launch.actions import Shutdown from launch import LaunchDescription from launch_ros.actions import Node @@ -21,13 +22,41 @@ from launch.substitutions import ThisLaunchFileDir from launch.substitutions import EnvironmentVariable from launch.actions import GroupAction +from launch.conditions import IfCondition from launch_ros.actions import PushRosNamespace from carma_ros2_utils.launch.get_log_level import GetLogLevel -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PythonExpression from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from tracetools_launch.action import Trace + +from datetime import datetime import os +def create_ros2_tracing_action(context, *args, **kwargs): + """ + Opaque Function for generating a 'Trace' ROS 2 launch action, which is dependent on the + 'ROS_LOG_DIR' EnvironmentVariable and the 'is_ros2_tracing_enabled' LaunchConfiguration. + + NOTE: This Opaque Function is required in order to evaluate the 'ROS_LOG_DIR' environment + variable as a string. + """ + log_directory_string = launch.substitutions.EnvironmentVariable('ROS_LOG_DIR').perform(context) + + trace = GroupAction( + condition=IfCondition(LaunchConfiguration('is_ros2_tracing_enabled')), + actions=[ + Trace( + base_path = log_directory_string, + session_name='my-tracing-session-' + str(datetime.now().strftime('%Y-%m-%d_%H%M%S')), + events_kernel = [], # Empty since kernel tracing is not enabled for CARMA Platform + ) + ] + ) + + return [trace] + def generate_launch_description(): """ Launch CARMA System. @@ -143,6 +172,25 @@ def generate_launch_description(): simulation_mode = LaunchConfiguration('simulation_mode') declare_simulation_mode = DeclareLaunchArgument(name='simulation_mode', default_value = 'False', description = 'True if CARMA Platform is launched with CARLA Simulator') + is_ros2_tracing_enabled = LaunchConfiguration('is_ros2_tracing_enabled') + declare_is_ros2_tracing_enabled = DeclareLaunchArgument( + name='is_ros2_tracing_enabled', + default_value = 'False', + description = 'True if user wants ROS 2 Tracing logs to be generated from CARMA Platform.') + + # Launch ROS2 rosbag logging + ros2_rosbag_launch = GroupAction( + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ros2_rosbag.launch.py']), + launch_arguments = { + 'vehicle_config_dir' : vehicle_config_dir, + 'vehicle_config_param_file' : vehicle_config_param_file + }.items() + ) + ] + ) + # Nodes transform_group = GroupAction( @@ -276,12 +324,15 @@ def generate_launch_description(): declare_arealist_path, declare_vector_map_file, declare_simulation_mode, + declare_is_ros2_tracing_enabled, drivers_group, transform_group, environment_group, localization_group, v2x_group, guidance_group, + ros2_rosbag_launch, ui_group, - system_controller + system_controller, + OpaqueFunction(function=create_ros2_tracing_action) ]) diff --git a/carma/launch/drivers.launch.py b/carma/launch/drivers.launch.py index 47a822e3c2..4fc021fd52 100644 --- a/carma/launch/drivers.launch.py +++ b/carma/launch/drivers.launch.py @@ -84,7 +84,7 @@ def generate_launch_description(): package='subsystem_controllers', name='drivers_controller', executable='drivers_controller', - parameters=[ subsystem_controller_default_param_file, subsystem_controller_param_file ], + parameters=[ subsystem_controller_default_param_file, subsystem_controller_param_file ], on_exit= Shutdown(), # Mark the subsystem controller as required arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)] ) diff --git a/carma/launch/environment.launch.py b/carma/launch/environment.launch.py index 45eed2ac74..e29e839f24 100644 --- a/carma/launch/environment.launch.py +++ b/carma/launch/environment.launch.py @@ -23,6 +23,7 @@ from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument +from pathlib import PurePath import os @@ -90,7 +91,8 @@ def generate_launch_description(): carma_wm_ctrl_param_file = os.path.join( get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml') - + cp_multiple_object_tracker_node_file = str(PurePath(get_package_share_directory("carma_cooperative_perception"), "config/cp_multiple_object_tracker_node.yaml")) + cp_host_vehicle_filter_node_file = str(PurePath(get_package_share_directory("carma_cooperative_perception"), "config/cp_host_vehicle_filter_node.yaml")) # lidar_perception_container contains all nodes for lidar based object perception # a failure in any one node in the chain would invalidate the rest of it, so they can all be @@ -284,7 +286,8 @@ def generate_launch_description(): ("incoming_mobility_path", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_path" ] ), ("incoming_psm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_psm" ] ), ("incoming_bsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm" ] ), - ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ) + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("external_objects", "fused_external_objects") ], parameters=[ motion_computation_param_file, @@ -399,6 +402,114 @@ def generate_launch_description(): ) ] ) + + # Cooperative Perception Stack + carma_cooperative_perception_container = ComposableNodeContainer( + package='carma_ros2_utils', # rclcpp_components + name='carma_cooperative_perception_container', + executable='carma_component_container_mt', + namespace= GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='carma_cooperative_perception', + plugin='carma_cooperative_perception::ExternalObjectListToDetectionListNode', + name='cp_external_object_list_to_detection_list_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cp_external_object_list_to_detection_list_node', env_log_levels) }, + ], + remappings=[ + ("input/georeference", "georeference"), + ("output/detections", "full_detection_list"), + ("input/external_objects", "external_objects"), + ], + parameters=[ + ] + ), + ComposableNode( + package='carma_cooperative_perception', + plugin='carma_cooperative_perception::ExternalObjectListToSdsmNode', + name='cp_external_object_list_to_sdsm_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cp_external_object_list_to_sdsm_node', env_log_levels) }, + ], + remappings=[ + ("input/georeference", "georeference"), + ("output/sdsms", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_sdsm" ] ), + ("input/pose_stamped", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("input/external_objects", "external_objects"), + ], + parameters=[ + ] + ), + ComposableNode( + package='carma_cooperative_perception', + plugin='carma_cooperative_perception::HostVehicleFilterNode', + name='cp_host_vehicle_filter_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cp_host_vehicle_filter_node', env_log_levels) }, + ], + remappings=[ + ("input/host_vehicle_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("input/detection_list", "full_detection_list"), + ("output/detection_list", "filtered_detection_list") + ], + parameters=[ + cp_host_vehicle_filter_node_file + ] + ), + ComposableNode( + package='carma_cooperative_perception', + plugin='carma_cooperative_perception::SdsmToDetectionListNode', + name='cp_sdsm_to_detection_list_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cp_sdsm_to_detection_list_node', env_log_levels) }, + ], + remappings=[ + ("input/sdsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_sdsm" ] ), + ("output/detections", "full_detection_list"), + ], + parameters=[ + ] + ), + ComposableNode( + package='carma_cooperative_perception', + plugin='carma_cooperative_perception::TrackListToExternalObjectListNode', + name='cp_track_list_to_external_object_list_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cp_track_list_to_external_object_list_node', env_log_levels) }, + ], + remappings=[ + ("input/track_list", "cooperative_perception_track_list"), + ("output/external_object_list", "fused_external_objects"), + ], + parameters=[ + ] + ), + ComposableNode( + package='carma_cooperative_perception', + plugin='carma_cooperative_perception::MultipleObjectTrackerNode', + name='cp_multiple_object_tracker_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cp_multiple_object_tracker_node', env_log_levels) }, + ], + remappings=[ + ("output/track_list", "cooperative_perception_track_list"), + ("input/detection_list", "filtered_detection_list"), + ], + parameters=[ + cp_multiple_object_tracker_node_file + ] + ), + + ] + ) + # subsystem_controller which orchestrates the lifecycle of this subsystem's components subsystem_controller = Node( package='subsystem_controllers', @@ -418,5 +529,6 @@ def generate_launch_description(): carma_external_objects_container, lanelet2_map_loader_container, lanelet2_map_visualization_container, + carma_cooperative_perception_container, subsystem_controller ]) diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py index a4b891335e..7c9d5d6652 100644 --- a/carma/launch/localization.launch.py +++ b/carma/launch/localization.launch.py @@ -30,7 +30,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import GroupAction from launch_ros.actions import set_remap -from launch.conditions import IfCondition +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration, PythonExpression @@ -80,6 +80,7 @@ def generate_launch_description(): declare_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='vector_map.osm', description='Path to the map osm file if using the noupdate load type') simulation_mode = LaunchConfiguration('simulation_mode') + declare_simulation_mode = DeclareLaunchArgument(name='simulation_mode', default_value = 'False', description = 'True if CARMA Platform is launched with CARLA Simulator') gnss_to_map_convertor_container = ComposableNodeContainer( package='carma_ros2_utils', @@ -239,7 +240,7 @@ def generate_launch_description(): # EKF Localizer # Comment out to remove and change marked line in waypoint following.launch ekf_localizer_container = ComposableNodeContainer( - condition=IfCondition(PythonExpression(['not ', simulation_mode])), + condition=UnlessCondition(simulation_mode), # not needed in simulation package='carma_ros2_utils', name='ekf_localizer_container', namespace=GetCurrentNamespace(), diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 14b13fa717..2dbf8bf905 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -380,7 +380,7 @@ def generate_launch_description(): remappings = [ ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), - ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ), + ("external_object_predictions", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/external_object_predictions" ] ), ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ), ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ), ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ), diff --git a/carma/launch/ros2_rosbag.launch.py b/carma/launch/ros2_rosbag.launch.py new file mode 100644 index 0000000000..9cbb8ae567 --- /dev/null +++ b/carma/launch/ros2_rosbag.launch.py @@ -0,0 +1,93 @@ +# 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. + +from launch import LaunchDescription, LaunchContext +from launch_ros.actions import Node +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, ExecuteProcess + +from datetime import datetime +import pathlib +import yaml + +# This function is used to generate a command to record a ROS 2 rosbag that excludes topics +# topics as provided in the appropriate configuration file. +def record_ros2_rosbag(context: LaunchContext, vehicle_config_param_file): + + # Convert LaunchConfiguration object to its string representation + vehicle_config_param_file_string = context.perform_substitution(vehicle_config_param_file) + + # Initialize string that will contain the regex for topics to exclude from the ROS 2 rosbag + exclude_topics_regex = "" + + # Open vehicle config params file to process various rosbag settings + with open(vehicle_config_param_file_string, 'r') as f: + + vehicle_config_params = yaml.safe_load(f) + + if "use_ros2_rosbag" in vehicle_config_params: + if vehicle_config_params["use_ros2_rosbag"] == True: + + if "exclude_default" in vehicle_config_params: + if (vehicle_config_params["exclude_default"] == True) and ("excluded_default_topics" in vehicle_config_params): + for topic in vehicle_config_params["excluded_default_topics"]: + exclude_topics_regex += str(topic) + "|" + + if "exclude_lidar" in vehicle_config_params: + if (vehicle_config_params["exclude_lidar"] == True) and ("excluded_lidar_topics" in vehicle_config_params): + for topic in vehicle_config_params["excluded_lidar_topics"]: + exclude_topics_regex += str(topic) + "|" + + if "exclude_camera" in vehicle_config_params: + if (vehicle_config_params["exclude_camera"] == True) and ("excluded_camera_topics" in vehicle_config_params): + for topic in vehicle_config_params["excluded_camera_topics"]: + exclude_topics_regex += str(topic) + "|" + + if "exclude_can" in vehicle_config_params: + if (vehicle_config_params["exclude_can"] == True) and ("excluded_can_topics" in vehicle_config_params): + for topic in vehicle_config_params["excluded_can_topics"]: + exclude_topics_regex += str(topic) + "|" + + proc = ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-o', '/opt/carma/logs/rosbag2_' + str(datetime.now().strftime('%Y-%m-%d_%H%M%S')), '-a', '-x', exclude_topics_regex], + output='screen', + shell='true' + ) + + return [proc] + + +def generate_launch_description(): + # Declare the vehicle_config_dir launch argument + vehicle_config_dir = LaunchConfiguration('vehicle_config_dir') + declare_vehicle_config_dir_arg = DeclareLaunchArgument( + name = 'vehicle_config_dir', + default_value = "/opt/carma/vehicle/config", + description = "Path to file containing vehicle config directories" + ) + + # Declare the vehicle_config_param_file launch argument + vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') + declare_vehicle_config_param_file_arg = DeclareLaunchArgument( + name = 'vehicle_config_param_file', + default_value = [vehicle_config_dir, "/VehicleConfigParams.yaml"], + description = "Path to file contain vehicle configuration parameters" + ) + + return LaunchDescription([ + declare_vehicle_config_dir_arg, + declare_vehicle_config_param_file_arg, + OpaqueFunction(function=record_ros2_rosbag, args=[LaunchConfiguration('vehicle_config_param_file')]) + ]) \ No newline at end of file diff --git a/carma_cooperative_perception/.clang-format b/carma_cooperative_perception/.clang-format new file mode 100644 index 0000000000..7098810770 --- /dev/null +++ b/carma_cooperative_perception/.clang-format @@ -0,0 +1,22 @@ +--- +# ROS 2 C++ style guidelines +# Reference: https://github.com/ament/ament_lint/blob/rolling/ament_clang_format/ament_clang_format/configuration/.clang-format +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterEnum: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +IncludeBlocks: Preserve diff --git a/carma_cooperative_perception/CMakeLists.txt b/carma_cooperative_perception/CMakeLists.txt new file mode 100644 index 0000000000..8627b63558 --- /dev/null +++ b/carma_cooperative_perception/CMakeLists.txt @@ -0,0 +1,157 @@ +# Copyright 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. + +cmake_minimum_required(VERSION 3.16) +project(carma_cooperative_perception) + +# CARMA builds packages in an environment with both ROS 1 and ROS 2 installed. +# This check gracefully skips the current package if the sourced ROS environment +# is not the specified version. This call must come before any other ROS +# dependencies becasue ROS 1 does not have some of the required packages. +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) + +# Added outside of `dependencies.cmake` because ament sets some variables +# (e.g., BUILD_TESTING) that affect the configuration options for the rest of +# the package. Putting the command call here allows us to put all project +# options together in separate CMake module then query those options in +# `dependencies.cmake`. +find_package(ament_cmake_auto REQUIRED) + +# Needed so CMake can find the vendored PROJ4 module file. Th FindPROJ4.cmake +# module file can be removed if we upgrade to a more recent PROJ version. See +# https://github.com/usdot-fhwa-stol/carma-platform/issues/2139 for updates. +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) + +include(cmake_options.cmake) +include(dependencies.cmake) + +# The generated compilation database is helpful with code completion in IDEs +set(CMAKE_EXPORT_COMPILE_COMMANDS ${carma_cooperative_perception_EXPORT_COMPILE_COMMANDS}) + +# This prevents `colcon` from tyring to build the CMake project's binary +# directory. This is useful in case we want to build the project outside of +# `colcon` but still keep it in a ROS repository. +file(TOUCH ${PROJECT_BINARY_DIR}/COLCON_IGNORE) + +# Configures CARMA package default settings +carma_package() + +# C17 CMake support added in CMake 3.21 +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +# This will automatically add include files from include/carma_cooperative_perception +ament_auto_add_library(carma_cooperative_perception SHARED + src/external_object_list_to_detection_list_component.cpp + src/external_object_list_to_sdsm_component.cpp + src/geodetic.cpp + src/j2735_types.cpp + src/j3224_types.cpp + src/msg_conversion.cpp + src/sdsm_to_detection_list_component.cpp + src/track_list_to_external_object_list_component.cpp + src/utm_zone.cpp + src/multiple_object_tracker_component.cpp + src/host_vehicle_filter_component.cpp +) + +target_link_libraries(carma_cooperative_perception + ${PROJ4_LIBRARIES} # Note: Newer versions of PROJ use PROJ::proj + GSL # Note: Newer versions of GSL use Microsoft.GSL::GSL + units::units + cooperative_perception_core::cooperative_perception_coreLibrary +) + +rclcpp_components_register_nodes(carma_cooperative_perception + "carma_cooperative_perception::ExternalObjectListToDetectionListNode" + "carma_cooperative_perception::ExternalObjectListToSdsmNode" + "carma_cooperative_perception::SdsmToDetectionListNode" + "carma_cooperative_perception::TrackListToExternalObjectListNode" + "carma_cooperative_perception::MultipleObjectTrackerNode" + "carma_cooperative_perception::HostVehicleFilterNode" +) + +ament_auto_add_executable(external_object_list_to_detection_list_node + src/external_object_list_to_detection_list_node.cpp +) + +ament_auto_add_executable(track_list_to_external_object_list_node + src/track_list_to_external_object_list_node.cpp +) + +ament_auto_add_executable(multiple_object_tracker_node + src/multiple_object_tracker_node.cpp +) + +ament_auto_add_executable(external_object_list_to_sdsm_node + src/external_object_list_to_sdsm_node.cpp +) + +ament_auto_add_executable(host_vehicle_filter_node + src/host_vehicle_filter_node.cpp +) + +# boost::posix_time definition for using nanoseconds +add_definitions(-DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG) + +if(carma_cooperative_perception_BUILD_TESTS) + enable_testing() + + # We must add tests to top-level CMakeLists.txt because the + # ament_auto_add_gtest command will look for headers in the include/ + # directory. Moving this command to test/CMakeLists.txt will prevent the + # header files from getting included. + ament_auto_add_gtest(carma_cooperative_perception_tests + test/test_external_object_list_to_detection_list_component.cpp + test/test_geodetic.cpp + test/test_j2735_types.cpp + test/test_j3224_types.cpp + test/test_month.cpp + test/test_msg_conversion.cpp + ) + + target_link_libraries(carma_cooperative_perception_tests + carma_cooperative_perception + ) + + add_launch_test(test/track_list_to_external_object_list_launch_test.py) + add_launch_test(test/multiple_object_tracker_duplicates_launch_test.py) + add_launch_test(test/multiple_object_tracker_static_obstacle_launch_test.py) + add_launch_test(test/host_vehicle_filter_launch_test.py) + +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) + +include(GNUInstallDirs) + +# ament_auto does not provide file pattern matching +install(DIRECTORY ${PROJECT_SOURCE_DIR}/test/ + DESTINATION ${CMAKE_INSTALL_DATADIR}/carma_cooperative_perception/test + FILES_MATCHING + PATTERN *.py + PATTERN *.cpp + PATTERN __pycache__ EXCLUDE +) + +install(DIRECTORY ${PROJECT_SOURCE_DIR}/test/data/ + DESTINATION ${CMAKE_INSTALL_DATADIR}/carma_cooperative_perception/test/data + FILES_MATCHING + PATTERN *.yaml +) diff --git a/carma_cooperative_perception/CPPLINT.cfg b/carma_cooperative_perception/CPPLINT.cfg new file mode 100644 index 0000000000..b874e7065b --- /dev/null +++ b/carma_cooperative_perception/CPPLINT.cfg @@ -0,0 +1,4 @@ +# Disable cpplint's include_order check. The checks are inconsistent with ROS 2 +# header files, so we are disabling it until we know more about how the order +# and C vs. C++ files are validated +filter=-build/include_order diff --git a/carma_cooperative_perception/README.md b/carma_cooperative_perception/README.md new file mode 100644 index 0000000000..0cc567aa96 --- /dev/null +++ b/carma_cooperative_perception/README.md @@ -0,0 +1,29 @@ +# CARMA cooperative perception stack + +The `carma_cooperative_perception` package contains the cooperative perception stack used in CARMA Platform. +It wraps the `cooperative_perception_core` library in a ROS 2 interface. The stack provides two main functionalities. +The multiple object tracking functionality aggregates object data from incoming Sensor Data Sharing Messages (SDSMs), +Basic Safety Messages (BSMs), and local sensors. The sensor data sharing functionality broadcasts SDSMs containing +object data generated by the host’s local sensors. Parts of the stack can be disabled to support CARMA Platform +vehicles with different sensor and computational capabilities. CARMA Platform deployments that include this package can +collaborate with other agents that also share object tracking information to compose a cooperative perception network. + +> [!IMPORTANT]\ +> **Current limitations:** The package currently does not support BSMs. + +## CMake options + +The table below contains package-specific CMake options. + +| Option | Default value | Description | +| ------------------------------------------ | ------------------ | -------------------------- | +| `carma_cooperative_perception_BUILD_TESTS` | `${BUILD_TESTING}` | Build the package's tests. | + +## Documentation + +For information about the package's nodes, launch configurations, and other ROS 2 specific items, check out the +[package design][package_design_link] page. For details about the CARMA cooperative perception stack in general, check +out the [stack design][stack_design_link] page. + +[package_design_link]: docs/package_design.md +[stack_design_link]: docs/stack_design.md diff --git a/carma_cooperative_perception/cmake/FindPROJ4.cmake b/carma_cooperative_perception/cmake/FindPROJ4.cmake new file mode 100644 index 0000000000..880668a4c3 --- /dev/null +++ b/carma_cooperative_perception/cmake/FindPROJ4.cmake @@ -0,0 +1,91 @@ +# BSD 3-Clause License +# +# Copyright (c) 2009, Mateusz Loskot +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +############################################################################### +# CMake module to search for PROJ.4 library +# +# On success, the macro sets the following variables: +# PROJ4_FOUND = if the library found +# PROJ4_VERSION = version number of PROJ.4 found +# PROJ4_LIBRARIES = full path to the library +# PROJ4_INCLUDE_DIR = where to find the library headers +# Also defined, but not for general use are +# PROJ4_LIBRARY, where to find the PROJ.4 library. +# +# Copyright (c) 2009 Mateusz Loskot +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. +# +############################################################################### + +# lint_cmake: -linelength +# Reference: https://raw.githubusercontent.com/mloskot/cmake-modules/master/modules/FindPROJ4.cmake + +# Try to use OSGeo4W installation +if(WIN32) + set(PROJ4_OSGEO4W_HOME "C:/OSGeo4W") + + if($ENV{OSGEO4W_HOME}) + set(PROJ4_OSGEO4W_HOME "$ENV{OSGEO4W_HOME}") + endif() +endif() + +find_path(PROJ4_INCLUDE_DIR proj_api.h + PATHS ${PROJ4_OSGEO4W_HOME}/include + DOC "Path to PROJ.4 library include directory") + +if(PROJ4_INCLUDE_DIR) + # Extract version from proj_api.h (ex: 480) + file(STRINGS ${PROJ4_INCLUDE_DIR}/proj_api.h + PJ_VERSIONSTR + REGEX "#define[ ]+PJ_VERSION[ ]+[0-9]+") + string(REGEX MATCH "[0-9]+" PJ_VERSIONSTR ${PJ_VERSIONSTR}) + + # TODO: Should be formatted as 4.8.0? + set(PROJ4_VERSION ${PJ_VERSIONSTR}) +endif() + +set(PROJ4_NAMES ${PROJ4_NAMES} proj proj_i) +find_library(PROJ4_LIBRARY + NAMES ${PROJ4_NAMES} + PATHS ${PROJ4_OSGEO4W_HOME}/lib + DOC "Path to PROJ.4 library file") + +if(PROJ4_LIBRARY) + set(PROJ4_LIBRARIES ${PROJ4_LIBRARY}) +endif() + +# Handle the QUIETLY and REQUIRED arguments and set PROJ4_FOUND to TRUE +# if all listed variables are TRUE +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PROJ4 DEFAULT_MSG + PROJ4_LIBRARY + PROJ4_INCLUDE_DIR) diff --git a/carma_cooperative_perception/cmake/ament_auto_add_gtest.cmake b/carma_cooperative_perception/cmake/ament_auto_add_gtest.cmake new file mode 100644 index 0000000000..f723f9c9bb --- /dev/null +++ b/carma_cooperative_perception/cmake/ament_auto_add_gtest.cmake @@ -0,0 +1,112 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# 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. + +# +# Add a gtest with all found test dependencies. +# +# Call add_executable(target ARGN), link it against the gtest libraries +# and all found test dependencies, and then register the executable as a test. +# +# If gtest is not available the specified target is not being created and +# therefore the target existence should be checked before being used. +# +# :param target: the target name which will also be used as the test name +# :type target: string +# :param ARGN: the list of source files +# :type ARGN: list of strings +# :param RUNNER: the path to the test runner script (default: +# see ament_add_test). +# :type RUNNER: string +# :param TIMEOUT: the test timeout in seconds, +# default defined by ``ament_add_test()`` +# :type TIMEOUT: integer +# :param WORKING_DIRECTORY: the working directory for invoking the +# executable in, default defined by ``ament_add_test()`` +# :type WORKING_DIRECTORY: string +# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest +# main libraries +# :type SKIP_LINKING_MAIN_LIBRARIES: option +# :param SKIP_TEST: if set mark the test as being skipped +# :type SKIP_TEST: option +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# +# @public +# +macro(ament_auto_add_gtest target) + cmake_parse_arguments(_ARG + "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + ${ARGN}) + if(NOT _ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR + "ament_auto_add_gtest() must be invoked with at least one source file") + endif() + + # add executable + set(_arg_executable ${_ARG_UNPARSED_ARGUMENTS}) + if(_ARG_SKIP_LINKING_MAIN_LIBRARIES) + list(APPEND _arg_executable "SKIP_LINKING_MAIN_LIBRARIES") + endif() + ament_add_gtest_executable("${target}" ${_arg_executable}) + + # add include directory of this package if it exists + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include") + target_include_directories("${target}" PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include") + endif() + + # link against other libraries of this package + if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "") + target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES}) + endif() + + # add exported information from found dependencies + ament_target_dependencies(${target} + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} + ) + + # add test + set(_arg_test "") + if(_ARG_RUNNER) + list(APPEND _arg_test "RUNNER" "${_ARG_RUNNER}") + endif() + if(_ARG_TIMEOUT) + list(APPEND _arg_test "TIMEOUT" "${_ARG_TIMEOUT}") + endif() + if(_ARG_WORKING_DIRECTORY) + list(APPEND _arg_test "WORKING_DIRECTORY" "${_ARG_WORKING_DIRECTORY}") + endif() + if(_ARG_SKIP_TEST) + list(APPEND _arg_test "SKIP_TEST") + endif() + if(_ARG_ENV) + list(APPEND _arg_test "ENV" ${_ARG_ENV}) + endif() + if(_ARG_APPEND_ENV) + list(APPEND _arg_test "APPEND_ENV" ${_ARG_APPEND_ENV}) + endif() + if(_ARG_APPEND_LIBRARY_DIRS) + list(APPEND _arg_test "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS}) + endif() + ament_add_gtest_test("${target}" ${_arg_test}) +endmacro() diff --git a/carma_cooperative_perception/cmake/ament_auto_find_test_dependencies.cmake b/carma_cooperative_perception/cmake/ament_auto_find_test_dependencies.cmake new file mode 100644 index 0000000000..8320b26e14 --- /dev/null +++ b/carma_cooperative_perception/cmake/ament_auto_find_test_dependencies.cmake @@ -0,0 +1,41 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# 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. + +# +# Invoke find_package() for all test dependencies. +# +# All found package names are appended to the +# ``${PROJECT_NAME}_FOUND_TEST_DEPENDS`` variables. +# +# @public +# +macro(ament_auto_find_test_dependencies) + set(_ARGN "${ARGN}") + if(_ARGN) + message(FATAL_ERROR "ament_auto_find_test_dependencies() called with " + "unused arguments: ${_ARGN}") + endif() + + if(NOT _AMENT_PACKAGE_NAME) + ament_package_xml() + endif() + + # try to find_package() all test dependencies + foreach(_dep ${${PROJECT_NAME}_TEST_DEPENDS}) + find_package(${_dep} QUIET) + if(${_dep}_FOUND) + list(APPEND ${PROJECT_NAME}_FOUND_TEST_DEPENDS ${_dep}) + endif() + endforeach() +endmacro() diff --git a/carma_cooperative_perception/cmake/get_cpm.cmake b/carma_cooperative_perception/cmake/get_cpm.cmake new file mode 100644 index 0000000000..5b5bc3e658 --- /dev/null +++ b/carma_cooperative_perception/cmake/get_cpm.cmake @@ -0,0 +1,58 @@ +# MIT License +# +# Copyright (c) 2019-2022 Lars Melchior and contributors +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +# lint_cmake: -linelength +# Reference: https://raw.githubusercontent.com/cpm-cmake/CPM.cmake/master/cmake/get_cpm.cmake + +set(CPM_DOWNLOAD_VERSION 0.38.2) + +if(CPM_SOURCE_CACHE) + set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +elseif(DEFINED ENV{CPM_SOURCE_CACHE}) + set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +else() + set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +endif() + +# Expand relative path. This is important if the provided path contains a tilde (~) +get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) + +function(download_cpm) + message(STATUS "Downloading CPM.cmake to ${CPM_DOWNLOAD_LOCATION}") + file(DOWNLOAD + https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake + ${CPM_DOWNLOAD_LOCATION} + ) +endfunction() + +if(NOT (EXISTS ${CPM_DOWNLOAD_LOCATION})) + download_cpm() +else() + # resume download if it previously failed + file(READ ${CPM_DOWNLOAD_LOCATION} check) + if("${check}" STREQUAL "") + download_cpm() + endif() + unset(check) +endif() + +include(${CPM_DOWNLOAD_LOCATION}) diff --git a/carma_cooperative_perception/cmake_options.cmake b/carma_cooperative_perception/cmake_options.cmake new file mode 100644 index 0000000000..2c40f69e7b --- /dev/null +++ b/carma_cooperative_perception/cmake_options.cmake @@ -0,0 +1,23 @@ +# Copyright 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. + +option(carma_cooperative_perception_BUILD_TESTS + "Build package tests" + ${BUILD_TESTING} +) + +option(carma_cooperative_perception_EXPORT_COMPILE_COMMANDS + "Export compile commands" + ON +) diff --git a/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml b/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml new file mode 100644 index 0000000000..2c2e5ca84d --- /dev/null +++ b/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml @@ -0,0 +1 @@ +distance_threshold_meters: 2.0 \ No newline at end of file diff --git a/carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml b/carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml new file mode 100644 index 0000000000..3068edd1ff --- /dev/null +++ b/carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml @@ -0,0 +1,3 @@ +execution_frequency_hz: 5.0 +track_promotion_threshold: 3 +track_removal_threshold: 0 \ No newline at end of file diff --git a/carma_cooperative_perception/dependencies.cmake b/carma_cooperative_perception/dependencies.cmake new file mode 100644 index 0000000000..46403a31cd --- /dev/null +++ b/carma_cooperative_perception/dependencies.cmake @@ -0,0 +1,71 @@ +# Copyright 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(cmake/get_cpm.cmake) +set(CPM_USE_LOCAL_PACKAGES ON) + +# lint_cmake: -readability/wonkycase +CPMAddPackage(NAME units + GITHUB_REPOSITORY nholthaus/units + GIT_TAG v2.3.3 + EXCLUDE_FROM_ALL ON + SYSTEM ON + OPTIONS + "BUILD_TESTS FALSE" + "BUILD_DOCS FALSE" +) + +find_package(cooperative_perception_core REQUIRED) + +# CARMA currently uses PROJ version 6.3.1, which is not designed to be incorporated +# as a subdirectory into larger projects. If CARMA upgrades to a newer version, we +# could use the CPMAddPackage(...) command to install PROJ as a source dependency +# if there is no version already locally available. +# See https://github.com/usdot-fhwa-stol/carma-platform/issues/2139 for the PROJ +# version upgrade plans. +find_package(PROJ4 REQUIRED MODULE) + +# lint_cmake: -readability/wonkycase +CPMAddPackage(NAME Microsoft.GSL + GITHUB_REPOSITORY microsoft/GSL + GIT_TAG v2.1.0 # This is the version shipped with Ubuntu 20.04; newer versions are available + EXCLUDE_FROM_ALL ON + SYSTEM ON + OPTIONS + "GSL_INSTALL TRUE" + "GSL_TEST FALSE" + "CMAKE_CXX_STANDARD 17" +) + +# This will pull dependencies from ... tags in the +# package.xml file. It saves us from having to manually call find_package(...) +# for each dependency. +ament_auto_find_build_dependencies() + +if(carma_cooperative_perception_BUILD_TESTS) + # These CMake commands were added to ament_cmake_auto in ROS 2 Humble. Until + # CARMA supports ROS 2 Humble, we will use package-local copies. + include(cmake/ament_auto_find_test_dependencies.cmake) + include(cmake/ament_auto_add_gtest.cmake) + + ament_auto_find_test_dependencies() + + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify # Using clang-format instead + ) + + set(ament_cmake_clang_format_CONFIG_FILE ${PROJECT_SOURCE_DIR}/.clang-format) + + ament_lint_auto_find_test_dependencies() +endif() diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_fusion_node_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_fusion_node_diagram.png new file mode 100644 index 0000000000..cf36b9d7be Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_fusion_node_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_fusion_system_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_fusion_system_diagram.png new file mode 100644 index 0000000000..00bf4fe6e7 Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_fusion_system_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_no_fusion_node_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_no_fusion_node_diagram.png new file mode 100644 index 0000000000..45f100e073 Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_no_fusion_node_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_no_fusion_system_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_no_fusion_system_diagram.png new file mode 100644 index 0000000000..4278dec047 Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_no_perception_no_fusion_system_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_fusion_node_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_fusion_node_diagram.png new file mode 100644 index 0000000000..7c0bcdd3fa Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_fusion_node_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_fusion_system_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_fusion_system_diagram.png new file mode 100644 index 0000000000..5c216a88b2 Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_fusion_system_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_no_fusion_node_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_no_fusion_node_diagram.png new file mode 100644 index 0000000000..b1a6c9ef42 Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_no_fusion_node_diagram.png differ diff --git a/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_no_fusion_system_diagram.png b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_no_fusion_system_diagram.png new file mode 100644 index 0000000000..97244a1374 Binary files /dev/null and b/carma_cooperative_perception/docs/assets/carma_cooperative_perception_perception_no_fusion_system_diagram.png differ diff --git a/carma_cooperative_perception/docs/external_object_list_to_detection_list_node.md b/carma_cooperative_perception/docs/external_object_list_to_detection_list_node.md new file mode 100644 index 0000000000..2df99f04b4 --- /dev/null +++ b/carma_cooperative_perception/docs/external_object_list_to_detection_list_node.md @@ -0,0 +1,36 @@ +# External object list to detection list Node + +This Node converts incoming `carma_perception_msgs/ExternalObjectList.msg` messages into +`carma_cooperative_perception_interfaces/DetectionList.msg` messages. It converts each external object’s position data +from the host vehicle's map frame to its UTM zone’s coordinate frame. + +## Subscriptions + +| Topic | Message Type | Description | +| -------------------------- | ---------------------------------------------- | ----------------------------------------------------------------------------------------------- | +| `~/input/external_objects` | `carma_perception_msgs/ExternalObjectList.msg` | Incoming external objects perceived locally | +| `~/input/georeference` | `std_msgs/String.msg` | PROJ string representing the projection from WGS-84 coordinates to the host vehicle's map frame | + +## Publishers + +| Topic | Message Type | Frequency | Description | +| --------------------- | ----------------------------------------------------------- | ------------------- | ------------------- | +| `~/output/detections` | `carma_cooperative_perception_interfaces/DetectionList.msg` | Subscription-driven | Outgoing detections | + +## Parameters + +| Topic | Data Type | Default Value | Required | Read Only | Description | +| ------------------------------ | --------- | --------------------------------------------------------------------------------------------- | -------- | --------- | ------------------------------------------------------------------------------------ | +| `~/unknown_motion_model` | `int64` | `3` (defined by `carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV`) | No | No | Motion model assigned to detected object types with an `UNKNOWN` semantic class | +| `~/small_vehicle_motion_model` | `int64` | `1` (defined by `carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV`) | No | No | Motion model assigned to detected object types with an `SMALL_VEHICLE` semantic class | +| `~/large_vehicle_motion_model` | `int64` | `1` (defined by `carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV`) | No | No | Motion model assigned to detected object types with an `LARGE_VEHICLE` semantic class | +| `~/motorcycle_motion_model` | `int64` | `2` (defined by `carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA`) | No | No | Motion model assigned to detected object types with an `MOTORCYCLE` semantic class | +| `~/pedestrian_motion_model` | `int64` | `3` (defined by `carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV`) | No | No | Motion model assigned to detected object types with an `PEDESTRIAN` semantic class | + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. diff --git a/carma_cooperative_perception/docs/external_object_list_to_sdsm_node.md b/carma_cooperative_perception/docs/external_object_list_to_sdsm_node.md new file mode 100644 index 0000000000..91cde404b4 --- /dev/null +++ b/carma_cooperative_perception/docs/external_object_list_to_sdsm_node.md @@ -0,0 +1,43 @@ +# External object list to SDSM Node + +This Node generates `carma_v2x_msgs/SensorDataSharingMessage.msg` messages from the +`carma_perception_msgs/ExternalObjectList.msg` messages that the host’s local perception stack outputs. It uses the +host vehicle's current position to convert the objects' positions into the appropriate coordinate frames. The host +vehicle's position becomes the reference point, and the objects' positions become offsets. This Node allows us to share +the tracking pipeline's outputs with neighboring vehicles and infrastructure, enabling and facilitating cooperative +perception. + +We don’t want to include data fused from incoming BSMs or SDSMs in our own SDSM because it would violate the implicit +assumption that measurements in SDSMs are independent among broadcasting actors. Quoting again a relevant portion from +SAE J3224 Section 5.2.1, + +> In the object detection state an HV or HRSU identifies objects in its field of view using its sensors and determines +> the static and dynamic characteristics of those detected objects. + +We interpret this to mean SDSMs should contain detection objects perceived only with an actor’s local capabilities. The +actor can perform object-level fusion before generating the SDSM, but the fusion process must involve only objects +detected with the host’s local perception capabilities. + +## Subscriptions + +| Topic | Message Type | Description | +| -------------------------- | ---------------------------------------------- | ------------------------------------------- | +| `~/input/external_objects` | `carma_perception_msgs/ExternalObjectList.msg` | Incoming external objects perceived locally | + +## Publishers + +| Topic | Message Type | Frequency | Description | +| ---------------- | --------------------------------------------- | ------------------- | --------------------------------------------- | +| `~/output/sdsms` | `carma_v2x_msgs/SensorDataSharingMessage.msg` | Subscription-driven | SDSMs generated from the external object list | + +## Parameters + +This node does provide parameters. + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. diff --git a/carma_cooperative_perception/docs/host_vehicle_filter_node.md b/carma_cooperative_perception/docs/host_vehicle_filter_node.md new file mode 100644 index 0000000000..e114a82562 --- /dev/null +++ b/carma_cooperative_perception/docs/host_vehicle_filter_node.md @@ -0,0 +1,37 @@ +# Host vehicle filter Node + +This Node removes detections that are likely to associate with the host vehicle. For each detection in the received +detection list, the Node will check the distance between it and the host vehicle's current pose. Any detections that +fall below the specified distance threshold will be pruned from the incoming detection list. The Node will then +republish the (possibly pruned) list. + +## Subscriptions + +| Topic | Message Type | Description | +| --------------------------- | --------------------------------------------------------------------------------- | ------------------------------- | +| `~/input/detection_list` | [`carma_cooperative_perception_interfaces/DetectionList.msg`][detection_list_msg] | Incoming detections | +| `~/input/host_vehicle_pose` | [`geometry_msgs/PoseStamped.msg`][pose_stamped_msg] | The host vehicle's current pose | + +[pose_stamped_msg]: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html + +## Publishers + +| Topic | Message Type | Frequency | Description | +| ------------------------- | --------------------------------------------------------------------------------- | ------------------- | -------------------------------------------------------------------------------------- | +| `~/output/detection_list` | [`carma_cooperative_perception_interfaces/DetectionList.msg`][detection_list_msg] | Subscription-driven | Incoming detections excluding any detections likely associating with the host vehicle. | + +## Parameters + +| Topic | Data Type | Default Value | Required | Read Only | Description | +| ---------------------- | --------- | ------------- | -------- | --------- | --------------------------------------------------------------------------------- | +| `~/distance_threshold_meters` | `float` | `0.0` | No | No | Distance below which a detection will be considered to represent the host vehicle | + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. + +[detection_list_msg]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/DetectionList.msg diff --git a/carma_cooperative_perception/docs/multiple_object_tracker_node.md b/carma_cooperative_perception/docs/multiple_object_tracker_node.md new file mode 100644 index 0000000000..23954ea869 --- /dev/null +++ b/carma_cooperative_perception/docs/multiple_object_tracker_node.md @@ -0,0 +1,55 @@ +# Multiple object tracker + +This is the main Node in the package; it does the actual object tracking. It wraps the `multiple_object_tracking` +library in a ROS 2 interface and composes the individual tracking steps into a cohesive pipeline. Check out the +library's documentation for details on the specific pipeline steps. For our purposes, we need to be concerned only with +the node's inputs and outputs. It receives incoming `carma_cooperative_perception_interfaces/DetectionList.msg` messages +from various sources (CARMA Platform and connected remote actors), and sends out +`carma_cooperative_perception_interfaces/TrackList.msg` messages. This Node fuses the data from the incoming +`DetectionList.msg` messages to form the outgoing `TrackList.msg` messages' data. + +## Node behavior + +The `multiple_object_tracker_node` Node executes a multiple object tracking pipeline at a specific frequency, which the +`execution_frequency_hz` parameter defines. Between executions, the Node listens for incoming `DetectionList.msg` +messages on the `~/input/detections` topic, queueing received detections for processsing. The tracking pipeline +executes the following steps on each iteration: + +1. temporal alignment +2. detection-to-track scoring +3. detection-to-track association +4. unassociated detection clustering +5. track maintenance +6. detection-to-track fusion + +The tracker Node outputs a list of confirmed tracks after executing the pipeline. + +## Subscriptions + +| Topic | Message Type | Description | +| -------------------- | -------------------------------------------------------------------------------------- | ------------------- | +| `~/input/detections` | [`carma_cooperative_perception_interfaces/DetectionList.msg`][detection_list_msg_link] | Incoming detections | + +[detection_list_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/DetectionList.msg + +## Publishers + +| Topic | Message Type | Frequency | Description | +| ----------------- | ------------------------------------------------------------------------------ | ----------------- | --------------------------------------------------------------------------------------------- | +| `~/output/tracks` | [`carma_cooperative_perception_interfaces/TrackList.msg`][track_list_msg_link] | Parameter-defined | Tracked objects from the pipeline. **Note:** The track list contains only _confirmed_ tracks. | + +[track_list_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/TrackList.msg + +## Parameters + +| Topic | Data Type | Default Value | Required | Read Only | Description | +| -------------------------- | --------- | ------------- | -------- | --------- | ------------------------------------------------------------ | +| `~/execution_frequency_hz` | `float` | `2.0` | No | No | Tracking execution pipeline's execution frequency (in Hertz) | + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. diff --git a/carma_cooperative_perception/docs/package_design.md b/carma_cooperative_perception/docs/package_design.md new file mode 100644 index 0000000000..6c0828ed15 --- /dev/null +++ b/carma_cooperative_perception/docs/package_design.md @@ -0,0 +1,37 @@ +# CARMA cooperative perception - ROS 2 package design + +This package provides the necessary ROS 2 Nodes and Launch files to support cooperative perception within CARMA +Platform. Message conversion Nodes allow the stack to ingest heterogeneous data sources by converting unique messages +types to a `carma_cooperative_perception/DetectionList.msg` message, which gets passed into a multiple-object tracking +pipeline. On the pipeline's output, more message conversion Nodes adapt the data to a format that is compatible with +the rest of CARMA Platform. + +## ROS 2 Nodes + +This package's ROS 2 Node composition is similar to a micro-services architecture. Let's call it a micro-node +architecture. Each Node in the package is responsible for a single task, either adapting a message type or tracking +objects. This architecture promotes testability and helps us decouple each concern's implementation. For example, the +main pipeline, the `multiple_object_tracker_node` Node, should not care about how message types get converted. The +subsystem's Nodes are managed (i.e., [ROS 2 Lifecycle Nodes][ros2_lifecycle_nodes_link]) and implemented as +[ROS 2 Components][ros2_components_link]. + +In its full capacity, the cooperative perception stack comprises two concurrent dataflows. One flow is responsible for +tracking and fusing object data from incoming basic safety message (BSMs), sensor data sharing messages (SDSMs), and +locally-perceived objects. This flow outputs only to the host; it does not share the results with the host’s neighbors. +The second dataflow fuses data coming from only locally-perceived objects, and the host shares the outputs from this +flow with its neighboring actors (e.g., other connected vehicles or infrastructure). + +### Package Nodes + +- Sensor data sharing message (SDSM) to detection list Node: [`sdsm_to_detection_list_node`][sdsm_to_detection_list_node_docs] +- External object list to detection list Node: + [`external_object_list_to_detection_list_node`][external_object_list_to_detection_list_node_docs] +- Track list to external object list Node: [`track_list_to_external_object_list_node`][track_list_to_external_object_list_node_docs] +- Multiple object tracker Node: [multiple_object_tracker_node_docs] + +[ros2_lifecycle_nodes_link]: https://design.ros2.org/articles/node_lifecycle.html +[ros2_components_link]: https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Composition.html +[sdsm_to_detection_list_node_docs]: sdsm_to_detection_list_node.md +[external_object_list_to_detection_list_node_docs]: external_object_list_to_detection_list_node.md +[track_list_to_external_object_list_node_docs]: track_list_to_external_object_list_node.md +[multiple_object_tracker_node_docs]: multiple_object_tracker_node.md diff --git a/carma_cooperative_perception/docs/sdsm_to_detection_list_node.md b/carma_cooperative_perception/docs/sdsm_to_detection_list_node.md new file mode 100644 index 0000000000..a0ed5d5e1c --- /dev/null +++ b/carma_cooperative_perception/docs/sdsm_to_detection_list_node.md @@ -0,0 +1,36 @@ +# SDSM to detection list Node + +This Node converts incoming `carma_v2x_msgs/SensorDataSharingMessage.msg` messages into +`carma_cooperative_perception/DetectionList.msg` messages. Similar to the Detection from basic safety message (BSM) +Node, this node performs some coordinate changes. The Node projects the SDSM's reference position from the WGS-84 datum +to the local UTM zone’s coordinate frame. It also converts the detected objects' positions, represented as Euclidean +offsets, to the same local UTM zone’s coordinate frame. + +## Subscriptions + +| Topic | Message Type | Description | +| -------------- | --------------------------------------------- | -------------- | +| `~/input/sdsm` | `carma_v2x_msgs/SensorDataSharingMessage.msg` | Incoming SDSMs | + +## Publishers + +| Topic | Message Type | Frequency | Description | +| --------------------- | ----------------------------------------------------------- | ------------------- | ------------------------ | +| `~/output/detections` | `carma_cooperative_perception_interfaces/DetectionList.msg` | Subscription-driven | Outgoing detection lists | + +## Parameters + +| Topic | Data Type | Default Value | Required | Read Only | Description | +| ------------------------ | --------- | ------------- | -------- | --------- | ------------------------------------------------------------------------------ | +| `~/unknown_motion_model` | `string` | `''` | Yes | Yes | Motion model assigned to detected object types with an `UNKNOWN` sematic class | +| `~/vehicle_motion_model` | `string` | `''` | Yes | Yes | Motion model assigned to detected object types with an `VEHICLE` sematic class | +| `~/vru_motion_model` | `string` | `''` | Yes | Yes | Motion model assigned to detected object types with an `VRU` sematic class | +| `~/animal_motion_model` | `string` | `''` | Yes | Yes | Motion model assigned to detected object types with an `ANIMAL` sematic class | + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. diff --git a/carma_cooperative_perception/docs/stack_design.md b/carma_cooperative_perception/docs/stack_design.md new file mode 100644 index 0000000000..a525e86c16 --- /dev/null +++ b/carma_cooperative_perception/docs/stack_design.md @@ -0,0 +1,83 @@ +# CARMA cooperative perception - Stack design + +The cooperative perception stack can take various forms depending on the deployed-to actor’s abilities. The subsections +below describe different configuration alternatives and what type of actors would most likely use them. For all of this +section’s figures, the gray rounded-boxes represent ROS 2 Nodes. Three main concepts distinguish the configurations: + +- **Local perception:** an actor can perceive the environment using only its equipped sensors. +- **Local fusion:** an actor can use its own processing capabilities to combine several data sources about an object + into a single representation. +- **External reliance:** an actor relies on some external entity to provide environment object information. These + actors lack local perception, local fusion, or both capabilities. + +The following table summarizes the different stack configurations. + +| Local perception | Local fusion | External reliance | +| ---------------- | ------------ | ----------------- | +| Yes | Yes | No | +| Yes | No | No | +| No | Yes | Depends | +| No | No | Yes | + +We assume all actors can receive incoming messages such as SDSMs, whether they can use that data is a different matter. +We also assume actors with local perception abilities have access to object-level data regarding the environment. + +## Local perception and fusion + +> [!IMPORTANT]\ +> This stack configuration is currently unsupported because the required Nodes are unimplemented. + +In this stack configuration variant, the host vehicle (HV) or host roadside unit (HRSU) can perceive objects using its +local sensors, and it can fuse remotely-received object data from BSMs and SDSMs. Actors using this configuration +broadcast sensor data sharing messages (SDSMs) containing locally-perceived objects. The figure below shows the Node +diagram for this deployment configuration. + +![](assets/carma_cooperative_perception_perception_fusion_node_diagram.png) +![](assets/carma_cooperative_perception_perception_fusion_system_diagram.png) + +## Local fusion only + +> [!IMPORTANT]\ +> This stack configuration is currently unsupported because the required Nodes are unimplemented. + +Host vehicles (HVs) or host roadside units (HRSUs) using this deployment configuration are unable to perceive the +environment using only their local abilities (i.e., their own sensors). However, they can fuse object information they +receive from nearby actors into single representations using their own computational power. The figure below shows the +Node diagram for this deployment configuration. + +Depending on the application or use case, actors with this stack configuration might rely on an external entity to +provide object data. If neighboring actors… + +![](assets/carma_cooperative_perception_no_perception_fusion_node_diagram.png) +![](assets/carma_cooperative_perception_no_perception_fusion_system_diagram.png) + +## Local Perception Only + +> [!IMPORTANT]\ +> This stack configuration is currently unsupported because the required Nodes are unimplemented. + +In this stack configuration, the host vehicle (HV) or host roadside unit (HRSU) can use object information only from +its local sensors; it cannot fuse that data with any remotely-received object data. Actors using this stack deployment +variant do not use incoming SDSMs, but they still publish their local sensor data with others through SDSMs. The figure +below shows the Node diagram for this deployment configuration. + +![](assets/carma_cooperative_perception_perception_no_fusion_node_diagram.png) +![](assets/carma_cooperative_perception_perception_no_fusion_system_diagram.png) + +## No Local Perception or Fusion + +> [!IMPORTANT]\ +> This stack configuration is currently unsupported because the required Nodes are unimplemented. + +> [!WARNING]\ +> We do not recommend this stack configuration but included it for completeness. If you intent to use this +> configuration in your application, we suggest you reevaluate your approach. + +In this stack configuration, the host vehicle (HV) or host roadside unit (HRSU) can neither perceive objects locally +nor fuse object data from nearby actors. The HV or HRSU is entirely dependent on an external system to provide an +authoritative list of environment objects. Essentially, actors with this configuration receive the all the cooperative +perception benefits without contributing themselves. The figure below shows the Node diagram for this deployment +configuration. + +![](assets/carma_cooperative_perception_no_perception_no_fusion_node_diagram.png) +![](assets/carma_cooperative_perception_no_perception_no_fusion_system_diagram.png) diff --git a/carma_cooperative_perception/docs/track_list_to_external_object_list_node.md b/carma_cooperative_perception/docs/track_list_to_external_object_list_node.md new file mode 100644 index 0000000000..d3af2b3053 --- /dev/null +++ b/carma_cooperative_perception/docs/track_list_to_external_object_list_node.md @@ -0,0 +1,43 @@ +# Track to external object list + +This Node generates `carma_perception_msgs/ExternalObjectList.msg` messages +from the tracking pipeline's outputted +`carma_cooperative_perception_interfaces/TrackList.msg` messages. We designed +`carma_cooperative_perception` package to be transparent to CARMA Platform, so +we need this conversion Node to inject the fused obstacle data back into CARMA +Platform's perception pipeline. + +> [!IMPORTANT]\ +> The `carma_cooperative_perception_interfaces/msg/Track.msg` messages store +> tracks' identifiers (IDs) as strings, but the +> `carma_perception_msgs/msg/ExternalObject.msg` messages use unsigned +> integers for the `id` field (which is different than the `bsm_id` field). If +> the string-to-integer conversion fails during message conversion, the +> resulting `ExternalObject`'s `id` field will be unpopulated. + +## Subscriptions + +| Topic | Message Type | Description | +| -------------------- | ------------------------------------------------------------------------- | --------------- | +| `~/input/track_list` | [`carma_cooperative_perception_interfaces/TrackList.msg`][track_list_msg] | Incoming tracks | + +## Publishers + +| Topic | Message Type | Frequency | Description | +| ------------------------------- | -------------------------------------------------------------------------- | ------------------- | -------------------------------------- | +| `~/output/external_object_list` | [`carma_perception_msgs/ExternalObjectList.msg`][external_object_list_msg] | Subscription-driven | External objects generated from tracks | + +## Parameters + +This Node does not provide parameters. + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. + +[track_list_msg]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/TrackList.msg +[external_object_list_msg]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_perception_msgs/msg/ExternalObjectList.msg diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/external_object_list_to_detection_list_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/external_object_list_to_detection_list_component.hpp new file mode 100644 index 0000000000..180e55afa7 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/external_object_list_to_detection_list_component.hpp @@ -0,0 +1,71 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__EXTERNAL_OBJECT_LIST_TO_DETECTION_LIST_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__EXTERNAL_OBJECT_LIST_TO_DETECTION_LIST_COMPONENT_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include "carma_cooperative_perception/msg_conversion.hpp" + +namespace carma_cooperative_perception +{ +auto transform_from_map_to_utm( + carma_cooperative_perception_interfaces::msg::DetectionList detection_list, + const std::string & map_origin) -> carma_cooperative_perception_interfaces::msg::DetectionList; + +class ExternalObjectListToDetectionListNode : public carma_ros2_utils::CarmaLifecycleNode +{ + using input_msg_type = carma_perception_msgs::msg::ExternalObjectList; + using input_msg_shared_pointer = typename input_msg_type::SharedPtr; + using output_msg_type = carma_cooperative_perception_interfaces::msg::DetectionList; + +public: + explicit ExternalObjectListToDetectionListNode(const rclcpp::NodeOptions & options); + + auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto publish_as_detection_list(const input_msg_type & msg) const -> void; + + auto update_georeference(const std_msgs::msg::String & msg) noexcept -> void; + +private: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_{nullptr}; + rclcpp::Subscription::SharedPtr external_objects_subscription_{nullptr}; + rclcpp::Subscription::SharedPtr georeference_subscription_{nullptr}; + std::string map_georeference_{""}; + MotionModelMapping motion_model_mapping_{}; + OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; +}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__EXTERNAL_OBJECT_LIST_TO_DETECTION_LIST_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/external_object_list_to_sdsm_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/external_object_list_to_sdsm_component.hpp new file mode 100644 index 0000000000..e3ec19aaa3 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/external_object_list_to_sdsm_component.hpp @@ -0,0 +1,75 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__EXTERNAL_OBJECT_LIST_TO_SDSM_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__EXTERNAL_OBJECT_LIST_TO_SDSM_COMPONENT_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace carma_cooperative_perception +{ +class ExternalObjectListToSdsmNode : public carma_ros2_utils::CarmaLifecycleNode +{ + using sdsm_msg_type = carma_v2x_msgs::msg::SensorDataSharingMessage; + using external_objects_msg_type = carma_perception_msgs::msg::ExternalObjectList; + using georeference_msg_type = std_msgs::msg::String; + + using pose_msg_type = geometry_msgs::msg::PoseStamped; + +public: + explicit ExternalObjectListToSdsmNode(const rclcpp::NodeOptions & options); + + auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto publish_as_sdsm(const external_objects_msg_type & msg) const -> void; + + auto update_georeference(const georeference_msg_type & proj_string) noexcept -> void; + + auto update_current_pose(const pose_msg_type & pose) noexcept -> void; + +private: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr sdsm_publisher_{nullptr}; + rclcpp::Subscription::SharedPtr external_objects_subscription_{ + nullptr}; + rclcpp::Subscription::SharedPtr georeference_subscription_{nullptr}; + rclcpp::Subscription::SharedPtr current_pose_subscription_{nullptr}; + + std::string map_georeference_{""}; + geometry_msgs::msg::PoseStamped current_pose_; + + std::shared_ptr map_projector_{nullptr}; + OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; +}; +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__EXTERNAL_OBJECT_LIST_TO_SDSM_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/geodetic.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/geodetic.hpp new file mode 100644 index 0000000000..afd99fff28 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/geodetic.hpp @@ -0,0 +1,189 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__GEODETIC_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__GEODETIC_HPP_ + +/** + * This file contains functions and helper structs to facilitate transforming + * WGS-84 coordinates to UTM ones. +*/ + +#include + +#include "carma_cooperative_perception/utm_zone.hpp" + +namespace carma_cooperative_perception +{ +/** + * @brief Represents a position using WGS-84 coordinates +*/ +struct Wgs84Coordinate +{ + units::angle::degree_t latitude; /** Decimal degrees [-180, 180]. */ + units::angle::degree_t longitude; /** Decimal degrees [-90, 90]. */ + units::length::meter_t elevation; /** With respect to the reference ellipsoid. */ +}; + +/** + * @brief Represents a position using UTM coordinates +*/ +struct UtmCoordinate +{ + UtmZone utm_zone; + units::length::meter_t easting; + units::length::meter_t northing; + units::length::meter_t elevation; /** With respect to the reference ellipsoid. */ +}; + +/** + * @brief Represent a displacement from a UTM coordinate +*/ +struct UtmDisplacement +{ + units::length::meter_t easting; + units::length::meter_t northing; + units::length::meter_t elevation; +}; + +/** + * @brief Addition-assignment operator overload + * + * @param[in] coordinate Position represented in UTM coordinates + * @param[in] displacement Displacement from coordinate + * + * @return Reference to the coordinate's updated position +*/ +inline constexpr auto operator+=( + UtmCoordinate & coordinate, const UtmDisplacement & displacement) noexcept -> UtmCoordinate & +{ + coordinate.easting += displacement.easting; + coordinate.northing += displacement.northing; + coordinate.elevation += displacement.elevation; + + return coordinate; +} + +/** + * @brief Addition operator overload + * + * @param[in] coordinate Position represented in UTM coordinates + * @param[in] displacement Displacement form coordinate + * + * @return A new UtmCoordinate representing the new position +*/ +inline constexpr auto operator+( + UtmCoordinate coordinate, const UtmDisplacement & displacement) noexcept -> UtmCoordinate +{ + return coordinate += displacement; +} + +/** + * @brief Addition operator overload + * + * @param[in] displacement Displacement from coordinate + * @param[in] coordinate Position represented in UTM coordinates + * + * @return A new UtmCoordinate representing the new position +*/ +inline constexpr auto operator+( + const UtmDisplacement & displacement, UtmCoordinate coordinate) noexcept -> UtmCoordinate +{ + return coordinate += displacement; +} + +/** + * @brief Subtraction-assignment operator overload + * + * @param[in] coordinate Position represented in UTM coordinates + * @param[in] displacement Displacement from coordinate + * + * @return Reference to the coordinate's updated position +*/ +inline constexpr auto operator-=( + UtmCoordinate & coordinate, const UtmDisplacement & displacement) noexcept -> UtmCoordinate & +{ + coordinate.easting += displacement.easting; + coordinate.northing += displacement.northing; + coordinate.elevation += displacement.elevation; + + return coordinate; +} + +/** + * @brief Subtraction operator overload + * + * @param[in] coordinate Position represented in UTM coordinates + * @param[in] displacement Displacement form coordinate + * + * @return A new UtmCoordinate representing the new position +*/ +inline constexpr auto operator-( + UtmCoordinate coordinate, const UtmDisplacement & displacement) noexcept -> UtmCoordinate +{ + return coordinate -= displacement; +} + +/** + * @brief Subtraction operator overload + * + * @param[in] displacement Displacement from coordinate + * @param[in] coordinate Position represented in UTM coordinates + * + * @return A new UtmCoordinate representing the new position +*/ +inline constexpr auto operator-( + const UtmDisplacement & displacement, UtmCoordinate coordinate) noexcept -> UtmCoordinate +{ + return coordinate -= displacement; +} + +/** + * @brief Get the UTM zone number from a WGS-84 coordinate + * + * Note: This function will not work for coordinates in the special UTM zones + * Svalbard and Norway. + * + * @param[in] coordinate WGS-84 coordinate + * + * @return The UTM zone containing the coordinate +*/ +auto calculate_utm_zone(const Wgs84Coordinate & coordinate) -> UtmZone; + +/** + * @brief Projects a Wgs84Coordinate to its corresponding UTM zone + * + * @param[in] coordinate Position represented in WGS-84 coordinates + * + * @return Coordinate's position represented in UTM coordinates +*/ +auto project_to_utm(const Wgs84Coordinate & coordinate) -> UtmCoordinate; + +/** + * @brief Calculate grid convergence at a given position + * + * This function calculates the grid convergence at a specific coordinate with respect to + * a specified UTM zone. Grid convergence is the angle between true north and grid north. + * + * @param[in] position Position represented in WGS-84 coordinates + * @param[in] zone The UTM zone + * + * @return Grid convergence angle +*/ +auto calculate_grid_convergence(const Wgs84Coordinate & position, const UtmZone & zone) + -> units::angle::degree_t; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__GEODETIC_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp new file mode 100644 index 0000000000..cc1613845e --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp @@ -0,0 +1,73 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__HOST_VEHICLE_FILTER_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__HOST_VEHICLE_FILTER_COMPONENT_HPP_ + +#include +#include +#include +#include + +namespace carma_cooperative_perception +{ + +class HostVehicleFilterNode : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + using carma_ros2_utils::CarmaLifecycleNode::CarmaLifecycleNode; + + auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto update_host_vehicle_pose(const geometry_msgs::msg::PoseStamped & msg) noexcept -> void; + + auto attempt_filter_and_republish( + carma_cooperative_perception_interfaces::msg::DetectionList msg) noexcept -> void; + +private: + rclcpp::Subscription::SharedPtr + detection_list_sub_{nullptr}; + + rclcpp::Subscription::SharedPtr host_vehicle_pose_sub_{nullptr}; + + rclcpp_lifecycle::LifecyclePublisher< + carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr detection_list_pub_{ + nullptr}; + + std::optional host_vehicle_pose_{std::nullopt}; + + OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; + + double squared_distance_threshold_meters_{0.0}; +}; + +auto euclidean_distance_squared( + const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b) noexcept -> double; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__HOST_VEHICLE_FILTER_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/j2735_types.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/j2735_types.hpp new file mode 100644 index 0000000000..52440c9e4d --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/j2735_types.hpp @@ -0,0 +1,99 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__J2735_TYPES_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__J2735_TYPES_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "carma_cooperative_perception/month.hpp" +#include "carma_cooperative_perception/units_extensions.hpp" + +namespace carma_cooperative_perception +{ +struct DDateTime +{ + std::optional year; + std::optional month; + std::optional day; + std::optional hour; + std::optional minute; + std::optional second; + std::optional time_zone_offset; + + [[nodiscard]] static auto from_msg(const j2735_v2x_msgs::msg::DDateTime & msg) noexcept + -> DDateTime; +}; + +struct AccelerationSet4Way +{ + units::acceleration::centi_meters_per_second_squared_t longitudinal; + units::acceleration::centi_meters_per_second_squared_t lateral; + units::acceleration::two_centi_standard_gravities_t vert; + units::angular_velocity::centi_degrees_per_second_t yaw_rate; + + [[nodiscard]] static auto from_msg(const j2735_v2x_msgs::msg::AccelerationSet4Way & msg) noexcept + -> AccelerationSet4Way; + + [[nodiscard]] static auto from_msg(const carma_v2x_msgs::msg::AccelerationSet4Way & msg) noexcept + -> AccelerationSet4Way; +}; + +struct Position3D +{ + units::angle::deci_micro_degrees_t latitude{0.0}; + units::angle::deci_micro_degrees_t longitude{0.0}; + std::optional elevation; + + [[nodiscard]] static auto from_msg(const j2735_v2x_msgs::msg::Position3D & msg) noexcept + -> Position3D; + + [[nodiscard]] static auto from_msg(const carma_v2x_msgs::msg::Position3D & msg) noexcept + -> Position3D; +}; + +struct Heading +{ + units::angle::eighth_deci_degrees_t heading; + + [[nodiscard]] static auto from_msg(const j2735_v2x_msgs::msg::Heading & heading) noexcept + -> Heading; + + [[nodiscard]] static auto from_msg(const carma_v2x_msgs::msg::Heading & heading) noexcept + -> Heading; +}; + +struct Speed +{ + units::velocity::two_centi_meters_per_second_t speed; + + [[nodiscard]] static auto from_msg(const j2735_v2x_msgs::msg::Speed & speed) noexcept -> Speed; + + [[nodiscard]] static auto from_msg(const carma_v2x_msgs::msg::Speed & speed) noexcept -> Speed; +}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__J2735_TYPES_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/j3224_types.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/j3224_types.hpp new file mode 100644 index 0000000000..b0997395e2 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/j3224_types.hpp @@ -0,0 +1,54 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__J3224_TYPES_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__J3224_TYPES_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace carma_cooperative_perception +{ +struct PositionOffsetXYZ +{ + units::length::decimeter_t offset_x{0.0}; + units::length::decimeter_t offset_y{0.0}; + std::optional offset_z; + + [[nodiscard]] static auto from_msg(const j3224_v2x_msgs::msg::PositionOffsetXYZ & msg) noexcept + -> PositionOffsetXYZ; + + [[nodiscard]] static auto from_msg(const carma_v2x_msgs::msg::PositionOffsetXYZ & msg) noexcept + -> PositionOffsetXYZ; +}; + +struct MeasurementTimeOffset +{ + units::time::millisecond_t measurement_time_offset; + + [[nodiscard]] static auto from_msg( + const j3224_v2x_msgs::msg::MeasurementTimeOffset & msg) noexcept -> MeasurementTimeOffset; + + [[nodiscard]] static auto from_msg( + const carma_v2x_msgs::msg::MeasurementTimeOffset & msg) noexcept -> MeasurementTimeOffset; +}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__J3224_TYPES_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/month.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/month.hpp new file mode 100644 index 0000000000..8ff2a27fab --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/month.hpp @@ -0,0 +1,250 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__MONTH_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__MONTH_HPP_ + +/** + * This file contains a Month class implementation that should be source-compatible + * with std::chrono::month. Until CARMA targets C++20, we will have to use this + * instead of the standard library. +*/ + +#include +#include + +namespace carma_cooperative_perception +{ +class Month +{ +public: + Month() = default; + + /** + * @brief Create a Month instance with the specified value + * + * @param[in] month_value Numerical value of the month; typically between [1, 12] + */ + constexpr explicit Month(std::uint8_t month_value) noexcept : month_value_{month_value} {} + + /** + * @brief Pre-increment operator overload + * + * @return Reference to Month instance being incremented + */ + constexpr auto operator++() noexcept -> Month & + { + constexpr auto jan_value{1U}; + constexpr auto dec_value{12U}; + + month_value_ = (month_value_ + 1) % (dec_value + 1); + + if (month_value_ == 0) { + month_value_ = jan_value; + } + + return *this; + } + + /** + * @brief Post-increment operator overload + * + * @param[in] _ Dummy parameter used to distinguish from the pre-increment operator + * + * @return Copy of Month instance before it was incremented + */ + constexpr auto operator++(int /* dummy parameter */) noexcept -> Month + { + Month previous{*this}; + ++(*this); + + return previous; + } + + /** + * @brief Pre-decrement operator overload + * + * @return Reference to Month instance being decremented + */ + constexpr auto operator--() noexcept -> Month & + { + constexpr auto dec_value{12U}; + + month_value_ = (month_value_ - 1 % (dec_value + 1)); + + if (month_value_ == 0) { + month_value_ = dec_value; + } + + return *this; + } + + /** + * @brief Post-decrement operator overload + * + * @param[in] _ Dummy parameter used to distinguish from the pre-decrement operator + * + * @return Copy of Month instance before it was decremented + */ + constexpr auto operator--(int /* dummy parameter */) noexcept -> Month + { + Month previous{*this}; + --(*this); + + return previous; + } + + /** + * @brief Conversion function to convert Month instance to unsigned type + */ + constexpr explicit operator unsigned() const noexcept + { + return static_cast(month_value_); + } + + /** + * @brief Checks if Month instance's value is within valid Gregorian calendar range + * + * @return true if Month instance's value is within [1, 12]; false otherwise + */ + [[nodiscard]] constexpr auto ok() const noexcept -> bool + { + constexpr auto jan_value{1U}; + constexpr auto dec_value{12U}; + + return month_value_ >= jan_value && month_value_ <= dec_value; + } + + /** + * @brief Compare exact equality between two Month instances + * + * @param[in] x First Month instance + * @param[in] y Second Month instance + * + * @return true is Month instances are equal; false otherwise + */ + friend constexpr auto operator==(const Month & x, const Month & y) noexcept -> bool + { + return x.month_value_ == y.month_value_; + } + + /** + * @brief Compare exact inequality between two Month instances + * + * @param[in] x First Month instance + * @param[in] y Second Month instance + * + * @return true is Month instances are not equal; false otherwise + */ + friend constexpr auto operator!=(const Month & x, const Month & y) noexcept -> bool + { + return !(x == y); + } + + /** + * @brief Check if one Month instance is less than another + * + * @param[in] x First Month instance + * @param[in] y Second Month instance + * + * @return true is x comes before y in the calendar; false otherwise + */ + friend constexpr auto operator<(const Month & x, const Month & y) noexcept -> bool + { + return x.month_value_ < y.month_value_; + } + + /** + * @brief Check if one Month instance is less than or equal to another + * + * @param[in] x First Month instance + * @param[in] y Second Month instance + * + * @return true is x comes before y in the calendar or if instances are equal; false otherwise + */ + friend constexpr auto operator<=(const Month & x, const Month & y) noexcept -> bool + { + return x < y || x == y; + } + + /** + * @brief Check if one Month instance is greater than another + * + * @param[in] x First Month instance + * @param[in] y Second Month instance + * + * @return true is x comes after y in the calendar; false otherwise + */ + friend constexpr auto operator>(const Month & x, const Month & y) noexcept -> bool + { + return x.month_value_ > y.month_value_; + } + + /** + * @brief Check if one Month instance is greater than or equal to another + * + * @param[in] x First Month instance + * @param[in] y Second Month instance + * + * @return true is x comes after y in the calendar or if instances are equal; false otherwise + */ + friend constexpr auto operator>=(const Month & x, const Month & y) noexcept -> bool + { + return x > y || x == y; + } + + /** + * @brief Output a string representation of Month instance to an output stream + * + * @tparam CharT Character type of the specified stream + * @tparam Traits Character traits of the specified stream + * + * @param[in,out] os Output stream being written to + * @param[in] m Month instance being written out + * + * @return Reference to specified output stream + */ + template + friend auto operator<<(std::basic_ostream & os, const Month & m) + -> std::basic_ostream & + { + static constexpr std::array abbreviations{"Jan.", "Feb.", "Mar.", "Apr.", "May", "Jun.", + "Jul.", "Aug.", "Sep.", "Oct.", "Nov.", "Dec."}; + if (!m.ok()) { + return os << static_cast(m) << " is not a valid month"; + } + + return os << abbreviations.at(static_cast(m) - 1); + } + +private: + std::uint8_t month_value_; +}; + +inline constexpr Month January{1}; +inline constexpr Month February{2}; +inline constexpr Month March{3}; +inline constexpr Month April{4}; +inline constexpr Month May{5}; +inline constexpr Month June{6}; +inline constexpr Month July{7}; +inline constexpr Month August{8}; +inline constexpr Month September{9}; +inline constexpr Month October{10}; +inline constexpr Month November{11}; +inline constexpr Month December{12}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__MONTH_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp new file mode 100644 index 0000000000..038d8c1240 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp @@ -0,0 +1,118 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__MSG_CONVERSION_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__MSG_CONVERSION_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "carma_cooperative_perception/geodetic.hpp" +#include "carma_cooperative_perception/j2735_types.hpp" +#include "carma_cooperative_perception/j3224_types.hpp" + +#include + +#include +#include + +#include + +#include +#include + +namespace carma_cooperative_perception +{ +auto to_time_msg(const DDateTime & d_date_time) noexcept -> builtin_interfaces::msg::Time; + +auto calc_detection_time_stamp(DDateTime d_date_time, const MeasurementTimeOffset & offset) noexcept + -> DDateTime; + +auto to_ddate_time_msg(const builtin_interfaces::msg::Time & builtin_time) noexcept + -> j2735_v2x_msgs::msg::DDateTime; + +auto calc_sdsm_time_offset( + const builtin_interfaces::msg::Time & external_object_list_time, + const builtin_interfaces::msg::Time & external_object_time) noexcept + -> carma_v2x_msgs::msg::MeasurementTimeOffset; + +auto to_position_msg(const UtmCoordinate & position_utm) noexcept -> geometry_msgs::msg::Point; + +auto heading_to_enu_yaw(const units::angle::degree_t & heading) noexcept -> units::angle::degree_t; + +auto calc_relative_position( + const geometry_msgs::msg::PoseStamped & current_pose, + const carma_v2x_msgs::msg::PositionOffsetXYZ & detected_object_data) noexcept + -> carma_v2x_msgs::msg::PositionOffsetXYZ; + +auto transform_pose_from_map_to_wgs84( + const geometry_msgs::msg::PoseStamped & source_pose, + const std::shared_ptr & map_projection) noexcept + -> carma_v2x_msgs::msg::Position3D; + +auto to_detection_list_msg(const carma_v2x_msgs::msg::SensorDataSharingMessage & sdsm) noexcept + -> carma_cooperative_perception_interfaces::msg::DetectionList; + +struct MotionModelMapping +{ + std::uint8_t small_vehicle_model; + std::uint8_t large_vehicle_model; + std::uint8_t motorcycle_model; + std::uint8_t pedestrian_model; + std::uint8_t unknown_model; +}; + +auto to_detection_msg( + const carma_perception_msgs::msg::ExternalObject & object, + const MotionModelMapping & motion_model_mapping) noexcept + -> carma_cooperative_perception_interfaces::msg::Detection; + +auto to_detection_list_msg( + const carma_perception_msgs::msg::ExternalObjectList & object_list, + const MotionModelMapping & motion_model_mapping) noexcept + -> carma_cooperative_perception_interfaces::msg::DetectionList; + +auto to_external_object_msg( + const carma_cooperative_perception_interfaces::msg::Track & track) noexcept + -> carma_perception_msgs::msg::ExternalObject; + +auto to_external_object_list_msg( + const carma_cooperative_perception_interfaces::msg::TrackList & track_list) noexcept + -> carma_perception_msgs::msg::ExternalObjectList; + +auto to_sdsm_msg( + const carma_perception_msgs::msg::ExternalObjectList & external_object_list, + const geometry_msgs::msg::PoseStamped & current_pose, + const std::shared_ptr & map_projection) noexcept + -> carma_v2x_msgs::msg::SensorDataSharingMessage; + +auto to_detected_object_data_msg( + const carma_perception_msgs::msg::ExternalObject & external_object, + const std::shared_ptr & map_projection) noexcept + -> carma_v2x_msgs::msg::DetectedObjectData; + +auto enu_orientation_to_true_heading( + double yaw, const lanelet::BasicPoint3d & obj_pose, + const std::shared_ptr & map_projection) noexcept + -> units::angle::degree_t; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__MSG_CONVERSION_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/multiple_object_tracker_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/multiple_object_tracker_component.hpp new file mode 100644 index 0000000000..927ed37901 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/multiple_object_tracker_component.hpp @@ -0,0 +1,85 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__MULTIPLE_OBJECT_TRACKER_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__MULTIPLE_OBJECT_TRACKER_COMPONENT_HPP_ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace carma_cooperative_perception +{ + +using Detection = + std::variant; +using Track = std::variant; + +auto make_detection(const carma_cooperative_perception_interfaces::msg::Detection & msg) + -> Detection; + +class MultipleObjectTrackerNode : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + explicit MultipleObjectTrackerNode(const rclcpp::NodeOptions & options); + + auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto store_new_detections( + const carma_cooperative_perception_interfaces::msg::DetectionList & msg) noexcept -> void; + + auto execute_pipeline() -> void; + +private: + rclcpp::Subscription::SharedPtr + detection_list_sub_{nullptr}; + + rclcpp_lifecycle::LifecyclePublisher< + carma_cooperative_perception_interfaces::msg::TrackList>::SharedPtr track_list_pub_{nullptr}; + + rclcpp::TimerBase::SharedPtr pipeline_execution_timer_{nullptr}; + + std::vector detections_; + std::unordered_map uuid_index_map_; + cooperative_perception::FixedThresholdTrackManager track_manager_{ + cooperative_perception::PromotionThreshold{3U}, cooperative_perception::RemovalThreshold{0U}}; + units::time::nanosecond_t execution_period_{1 / units::frequency::hertz_t{2.0}}; + OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; +}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__MULTIPLE_OBJECT_TRACKER_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp new file mode 100644 index 0000000000..a805b079ba --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp @@ -0,0 +1,54 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__SDSM_TO_DETECTION_LIST_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__SDSM_TO_DETECTION_LIST_COMPONENT_HPP_ + +#include +#include +#include +#include + +#include "carma_cooperative_perception/msg_conversion.hpp" + +namespace carma_cooperative_perception +{ +class SdsmToDetectionListNode : public carma_ros2_utils::CarmaLifecycleNode +{ + using input_msg_type = carma_v2x_msgs::msg::SensorDataSharingMessage; + using input_msg_shared_pointer = typename input_msg_type::SharedPtr; + using output_msg_type = carma_cooperative_perception_interfaces::msg::DetectionList; + +public: + explicit SdsmToDetectionListNode(const rclcpp::NodeOptions & options) + : CarmaLifecycleNode{options}, + publisher_{create_publisher("output/detections", 1)}, + subscription_{create_subscription( + "input/sdsm", 1, [this](input_msg_shared_pointer msg_ptr) { sdsm_msg_callback(*msg_ptr); })} + { + } + + auto sdsm_msg_callback(const input_msg_type & msg) const noexcept -> void + { + publisher_->publish(to_detection_list_msg(msg)); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; +}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__SDSM_TO_DETECTION_LIST_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/track_list_to_external_object_list_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/track_list_to_external_object_list_component.hpp new file mode 100644 index 0000000000..b1cefb1fe2 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/track_list_to_external_object_list_component.hpp @@ -0,0 +1,60 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__TRACK_LIST_TO_EXTERNAL_OBJECT_LIST_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__TRACK_LIST_TO_EXTERNAL_OBJECT_LIST_COMPONENT_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include + +#include "carma_cooperative_perception/msg_conversion.hpp" + +namespace carma_cooperative_perception +{ +class TrackListToExternalObjectListNode : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + explicit TrackListToExternalObjectListNode(const rclcpp::NodeOptions & options); + + auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto publish_as_external_object_list( + const carma_cooperative_perception_interfaces::msg::TrackList & msg) const noexcept -> void; + +private: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + publisher_{nullptr}; + rclcpp::Subscription::SharedPtr + track_list_subscription_{nullptr}; + std::string map_georeference_{""}; + OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; +}; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__TRACK_LIST_TO_EXTERNAL_OBJECT_LIST_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/units_extensions.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/units_extensions.hpp new file mode 100644 index 0000000000..c212ed1f6b --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/units_extensions.hpp @@ -0,0 +1,81 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__UNITS_EXTENSIONS_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__UNITS_EXTENSIONS_HPP_ + +#include + +#include + +namespace carma_cooperative_perception +{ +template +constexpr auto remove_units(const T & value) noexcept +{ + return units::unit_cast(value); +} + +} // namespace carma_cooperative_perception + +/* + * The nholthaus/units library does not include an exhaustive list of units, so + * this is how we can add missing/new ones. See the following for more information: + * - https://github.com/nholthaus/units?tab=readme-ov-file#defining-new-units + * - https://github.com/nholthaus/units?tab=readme-ov-file#unit-definition-macros + */ +namespace units +{ +// These are not our macros, so we should not worry about linting them. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. Note also that +// clang-tidy release 14.0.0 adds NOLINTBEGIN...NOLINTEND, so we can remove the individual NOLINT +// calls in the future. + +UNIT_ADD( // NOLINT + acceleration, centi_meters_per_second_squared, centimeters_per_second_squared, // NOLINT + centi_mps_sq, // NOLINT + unit) // NOLINT + +UNIT_ADD( // NOLINT + acceleration, two_centi_standard_gravities, two_centi_standard_gravities, two_centi_SG, // NOLINT + unit, std::centi>, standard_gravity>) // NOLINT + +UNIT_ADD( // NOLINT + angular_velocity, centi_degrees_per_second, centi_degrees_per_second, centi_deg_per_s, // NOLINT + unit) // NOLINT + +UNIT_ADD( // NOLINT + angle, deci_micro_degrees, deci_micro_degrees, deci_udeg, // NOLINT + unit, degrees>) // NOLINT + +UNIT_ADD( // NOLINT + length, deca_centimeters, deca_centimeters, deca_cm, unit, centimeters>) // NOLINT + +UNIT_ADD( // NOLINT + angle, eighth_deci_degrees, eighth_deci_degrees, eighth_ddeg, // NOLINT + unit, std::deci>, degrees>) // NOLINT + +UNIT_ADD( // NOLINT + velocity, two_milli_meters_per_second, two_milli_meters_per_second, two_milli_mps, // NOLINT + unit, std::milli>, meters_per_second>) // NOLINT + +UNIT_ADD( // NOLINT + velocity, two_centi_meters_per_second, two_centi_meters_per_second, two_centi_mps, // NOLINT + unit, std::centi>, meters_per_second>) // NOLINT + +} // namespace units + +#endif // CARMA_COOPERATIVE_PERCEPTION__UNITS_EXTENSIONS_HPP_ diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/utm_zone.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/utm_zone.hpp new file mode 100644 index 0000000000..22ddaa6a64 --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/utm_zone.hpp @@ -0,0 +1,49 @@ +// Copyright 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. + +#ifndef CARMA_COOPERATIVE_PERCEPTION__UTM_ZONE_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__UTM_ZONE_HPP_ + +#include +#include + +namespace carma_cooperative_perception +{ +enum class Hemisphere +{ + kNorth, + kSouth +}; + +struct UtmZone +{ + std::size_t number; + Hemisphere hemisphere; +}; + +constexpr inline auto operator==(const UtmZone & lhs, const UtmZone & rhs) -> bool +{ + return lhs.number == rhs.number && lhs.hemisphere == rhs.hemisphere; +} + +constexpr inline auto operator!=(const UtmZone & lhs, const UtmZone & rhs) -> bool +{ + return !(lhs == rhs); +} + +auto to_string(const UtmZone & zone) -> std::string; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__UTM_ZONE_HPP_ diff --git a/carma_cooperative_perception/launch/cooperative_perception_launch.py b/carma_cooperative_perception/launch/cooperative_perception_launch.py new file mode 100644 index 0000000000..c14c554f7c --- /dev/null +++ b/carma_cooperative_perception/launch/cooperative_perception_launch.py @@ -0,0 +1,37 @@ +# Copyright 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. + +from ament_index_python import get_package_share_path +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + package_share_path = get_package_share_path("carma_cooperative_perception") + + multiple_object_tracker_node = Node( + package="carma_cooperative_perception", + executable="multiple_object_tracker_node", + name="multiple_object_tracker", + parameters=[package_share_path / "config/params.yaml"], + ) + + host_vehicle_filter_node = Node( + package="carma_cooperative_perception", + executable="host_vehicle_filter_node", + name="host_vehicle_filter", + parameters=[package_share_path / "config/params.yaml"], + ) + + return LaunchDescription([multiple_object_tracker_node, host_vehicle_filter_node]) diff --git a/carma_cooperative_perception/package.xml b/carma_cooperative_perception/package.xml new file mode 100644 index 0000000000..50dce8584b --- /dev/null +++ b/carma_cooperative_perception/package.xml @@ -0,0 +1,55 @@ + + + + carma_cooperative_perception + 0.1.0 + The CARMA Platform cooperative perception package + + carma + + Apache 2.0 + + ament_cmake + + ament_cmake_auto + carma_cmake_common + carma_cooperative_perception_interfaces + carma_perception_msgs + carma_ros2_utils + j2735_v2x_msgs + j3224_v2x_msgs + carma_v2x_msgs + rclcpp + lanelet2_core + lanelet2_extension + tf2_geometry_msgs + multiple_object_tracking + + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + ament_cmake_gtest + carma_launch_testing + carma_message_utilities + launch_testing + launch_testing_ament_cmake + rclcpy + + + ament_cmake + + diff --git a/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp b/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp new file mode 100644 index 0000000000..eec623d1d5 --- /dev/null +++ b/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp @@ -0,0 +1,217 @@ +// Copyright 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 "carma_cooperative_perception/external_object_list_to_detection_list_component.hpp" + +#include +#include +#include +#include + +#include "carma_cooperative_perception/geodetic.hpp" +#include "carma_cooperative_perception/units_extensions.hpp" + +namespace carma_cooperative_perception +{ +auto transform_from_map_to_utm( + carma_cooperative_perception_interfaces::msg::DetectionList detection_list, + const std::string & map_origin) -> carma_cooperative_perception_interfaces::msg::DetectionList +{ + gsl::owner context = proj_context_create(); + proj_log_level(context, PJ_LOG_NONE); + + if (context == nullptr) { + const std::string error_string{proj_errno_string(proj_context_errno(context))}; + throw std::invalid_argument("Could not create PROJ context: " + error_string + '.'); + } + + gsl::owner map_transformation = proj_create(context, map_origin.c_str()); + + if (map_transformation == nullptr) { + const std::string error_string{proj_errno_string(proj_context_errno(context))}; + throw std::invalid_argument("Could not create PROJ transform: " + error_string + '.'); + } + + std::vector new_detections; + for (auto detection : detection_list.detections) { + // Coordinate order is easting (meters), northing (meters) + const auto position_planar{ + proj_coord(detection.pose.pose.position.x, detection.pose.pose.position.y, 0, 0)}; + const auto proj_inverse{proj_trans(map_transformation, PJ_DIRECTION::PJ_INV, position_planar)}; + const Wgs84Coordinate position_wgs84{ + units::angle::radian_t{proj_inverse.lp.phi}, units::angle::radian_t{proj_inverse.lp.lam}, + units::length::meter_t{detection.pose.pose.position.z}}; + + const auto utm_zone{calculate_utm_zone(position_wgs84)}; + const auto position_utm{project_to_utm(position_wgs84)}; + + detection.header.frame_id = to_string(utm_zone); + detection.pose.pose.position.x = remove_units(position_utm.easting); + detection.pose.pose.position.y = remove_units(position_utm.northing); + + new_detections.push_back(std::move(detection)); + } + + std::swap(detection_list.detections, new_detections); + + proj_destroy(map_transformation); + proj_context_destroy(context); + + return detection_list; +} + +ExternalObjectListToDetectionListNode::ExternalObjectListToDetectionListNode( + const rclcpp::NodeOptions & options) +: CarmaLifecycleNode{options} +{ + lifecycle_publishers_.push_back(publisher_); + param_callback_handles_.push_back(on_set_parameters_callback_); +} + +auto ExternalObjectListToDetectionListNode::handle_on_configure( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + publisher_ = create_publisher("output/detections", 1); + + external_objects_subscription_ = create_subscription( + "input/external_objects", 1, [this](input_msg_shared_pointer msg_ptr) { + const auto current_state{this->get_current_state().label()}; + + if (current_state == "active") { + publish_as_detection_list(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->external_objects_subscription_->get_topic_name(), current_state.c_str()); + } + }); + + georeference_subscription_ = create_subscription( + "input/georeference", 1, [this](std_msgs::msg::String::SharedPtr msg_ptr) { + const auto current_state{this->get_current_state().label()}; + + if (current_state == "active") { + update_georeference(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->georeference_subscription_->get_topic_name(), current_state.c_str()); + } + }); + + declare_parameter( + "small_vehicle_motion_model", + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV); + + declare_parameter( + "large_vehicle_motion_model", + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV); + + declare_parameter( + "motorcycle_motion_model", + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA); + + declare_parameter( + "pedestrian_motion_model", + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV); + + declare_parameter( + "unknown_motion_model", + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV); + + on_set_parameters_callback_ = + add_on_set_parameters_callback([this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "small_vehicle_motion_model") { + this->motion_model_mapping_.small_vehicle_model = + static_cast(parameter.as_int()); + } else if (parameter.get_name() == "large_vehicle_motion_model") { + this->motion_model_mapping_.large_vehicle_model = + static_cast(parameter.as_int()); + } else if (parameter.get_name() == "motorcycle_motion_model") { + this->motion_model_mapping_.motorcycle_model = + static_cast(parameter.as_int()); + } else if (parameter.get_name() == "pedestrian_motion_model") { + this->motion_model_mapping_.pedestrian_model = + static_cast(parameter.as_int()); + } else if (parameter.get_name() == "unknown_motion_model") { + this->motion_model_mapping_.unknown_model = static_cast(parameter.as_int()); + } else { + result.successful = false; + result.reason = "Unexpected parameter name '" + parameter.get_name() + '\''; + } + } + + return result; + }); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto ExternalObjectListToDetectionListNode::handle_on_cleanup( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + external_objects_subscription_.reset(); + georeference_subscription_.reset(); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto ExternalObjectListToDetectionListNode::handle_on_shutdown( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + external_objects_subscription_.reset(); + georeference_subscription_.reset(); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto ExternalObjectListToDetectionListNode::publish_as_detection_list( + const input_msg_type & msg) const -> void +{ + try { + const auto detection_list{transform_from_map_to_utm( + to_detection_list_msg(msg, motion_model_mapping_), map_georeference_)}; + + publisher_->publish(detection_list); + } catch (const std::invalid_argument & e) { + RCLCPP_ERROR( + this->get_logger(), "Could not convert external object list to detection list: %s", e.what()); + } +} + +auto ExternalObjectListToDetectionListNode::update_georeference( + const std_msgs::msg::String & msg) noexcept -> void +{ + map_georeference_ = msg.data; +} + +} // namespace carma_cooperative_perception + +// This is not our macro, so we should not worry about linting it. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. +RCLCPP_COMPONENTS_REGISTER_NODE( // NOLINT + carma_cooperative_perception::ExternalObjectListToDetectionListNode) // NOLINT diff --git a/carma_cooperative_perception/src/external_object_list_to_detection_list_node.cpp b/carma_cooperative_perception/src/external_object_list_to_detection_list_node.cpp new file mode 100644 index 0000000000..4fc064eaec --- /dev/null +++ b/carma_cooperative_perception/src/external_object_list_to_detection_list_node.cpp @@ -0,0 +1,35 @@ +// Copyright 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 + +#include + +#include + +auto main(int argc, char * argv[]) noexcept -> int +{ + rclcpp::init(argc, argv); + + auto node{std::make_shared( + rclcpp::NodeOptions{})}; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp b/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp new file mode 100644 index 0000000000..c12c9004f5 --- /dev/null +++ b/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp @@ -0,0 +1,151 @@ +// Copyright 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 "carma_cooperative_perception/external_object_list_to_sdsm_component.hpp" + +#include +#include + +#include "carma_cooperative_perception/msg_conversion.hpp" +#include + +namespace carma_cooperative_perception +{ +ExternalObjectListToSdsmNode::ExternalObjectListToSdsmNode(const rclcpp::NodeOptions & options) +: CarmaLifecycleNode{options} +{ + lifecycle_publishers_.push_back(sdsm_publisher_); + param_callback_handles_.push_back(on_set_parameters_callback_); +} + +auto ExternalObjectListToSdsmNode::handle_on_configure( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Life cycle state transition: configuring"); + + sdsm_publisher_ = create_publisher("output/sdsms", 1); + + external_objects_subscription_ = create_subscription( + "input/external_objects", 1, [this](const external_objects_msg_type::SharedPtr msg_ptr) { + const auto current_state{this->get_current_state().label()}; + + if (current_state == "active") { + publish_as_sdsm(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->georeference_subscription_->get_topic_name(), current_state.c_str()); + } + }); + + georeference_subscription_ = create_subscription( + "input/georeference", 1, [this](const georeference_msg_type::SharedPtr msg_ptr) { + const auto current_state{this->get_current_state().label()}; + + if (current_state == "active") { + update_georeference(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not " + "activated. " + "Current node state: '%s'", + this->georeference_subscription_->get_topic_name(), current_state.c_str()); + } + }); + + current_pose_subscription_ = create_subscription( + "input/pose_stamped", 1, [this](const pose_msg_type::SharedPtr msg_ptr) { + const auto current_state{this->get_current_state().label()}; + + if (current_state == "active") { + update_current_pose(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to recieve message on topic '%s', but the containing node is not activated." + "current node state: '%s'", + this->current_pose_subscription_->get_topic_name(), current_state.c_str()); + } + }); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto ExternalObjectListToSdsmNode::handle_on_cleanup( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Life cycle state transition: cleaning up"); + + sdsm_publisher_.reset(); + external_objects_subscription_.reset(); + georeference_subscription_.reset(); + + current_pose_subscription_.reset(); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto ExternalObjectListToSdsmNode::handle_on_shutdown( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Life cycle state transition: shutting down"); + + sdsm_publisher_.reset(); + external_objects_subscription_.reset(); + georeference_subscription_.reset(); + + current_pose_subscription_.reset(); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto ExternalObjectListToSdsmNode::publish_as_sdsm(const external_objects_msg_type & msg) const + -> void +{ + try { + const auto sdsm_output{to_sdsm_msg(msg, current_pose_, map_projector_)}; + + sdsm_publisher_->publish(sdsm_output); + } catch (const std::invalid_argument & e) { + RCLCPP_ERROR( + this->get_logger(), "Could not convert external object list to SDSM: %s", e.what()); + } +} + +auto ExternalObjectListToSdsmNode::update_georeference(const georeference_msg_type & msg) noexcept + -> void +{ + map_georeference_ = msg.data; + map_projector_ = std::make_shared(msg.data.c_str()); +} + +auto ExternalObjectListToSdsmNode::update_current_pose(const pose_msg_type & msg) noexcept + -> void +{ + current_pose_ = msg; +} + +} // namespace carma_cooperative_perception + +// This is not our macro, so we should not worry about linting it. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. +RCLCPP_COMPONENTS_REGISTER_NODE( // NOLINT + carma_cooperative_perception::ExternalObjectListToSdsmNode) // NOLINT diff --git a/carma_cooperative_perception/src/external_object_list_to_sdsm_node.cpp b/carma_cooperative_perception/src/external_object_list_to_sdsm_node.cpp new file mode 100644 index 0000000000..010e145cbc --- /dev/null +++ b/carma_cooperative_perception/src/external_object_list_to_sdsm_node.cpp @@ -0,0 +1,35 @@ +// Copyright 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 + +#include + +#include + +auto main(int argc, char * argv[]) noexcept -> int +{ + rclcpp::init(argc, argv); + + auto node{std::make_shared( + rclcpp::NodeOptions{})}; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/carma_cooperative_perception/src/geodetic.cpp b/carma_cooperative_perception/src/geodetic.cpp new file mode 100644 index 0000000000..c8e492df98 --- /dev/null +++ b/carma_cooperative_perception/src/geodetic.cpp @@ -0,0 +1,131 @@ +// Copyright 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 "carma_cooperative_perception/geodetic.hpp" + +#include +#include + +#include +#include + +#include "carma_cooperative_perception/units_extensions.hpp" + +namespace carma_cooperative_perception +{ +auto calculate_utm_zone(const Wgs84Coordinate & coordinate) -> UtmZone +{ + // Note: std::floor prevents this function from being constexpr (until C++23) + + static constexpr std::size_t zone_width{6}; + static constexpr std::size_t max_zones{60}; + + // Works for longitudes [-180, 360). Longitude of 360 will assign 61. + const auto number{ + static_cast( + (std::floor(carma_cooperative_perception::remove_units(coordinate.longitude) + 180) / + zone_width)) + + 1}; + + UtmZone zone; + + // std::min is used to handle the "UTM Zone 61" case. + zone.number = std::min(number, max_zones); + + if (coordinate.latitude < units::angle::degree_t{0.0}) { + zone.hemisphere = Hemisphere::kSouth; + } else { + zone.hemisphere = Hemisphere::kNorth; + } + + return zone; +} + +auto project_to_utm(const Wgs84Coordinate & coordinate) -> UtmCoordinate +{ + gsl::owner context = proj_context_create(); + proj_log_level(context, PJ_LOG_NONE); + + if (context == nullptr) { + const std::string error_string{proj_errno_string(proj_context_errno(context))}; + throw std::invalid_argument("Could not create PROJ context: " + error_string + '.'); + } + + const auto utm_zone{calculate_utm_zone(coordinate)}; + std::string proj_string{"+proj=utm +zone=" + std::to_string(utm_zone.number) + " +datum=WGS84"}; + + if (utm_zone.hemisphere == Hemisphere::kSouth) { + proj_string += " +south"; + } + + gsl ::owner utm_transformation = + proj_create_crs_to_crs(context, "EPSG:4326", proj_string.c_str(), nullptr); + + if (utm_transformation == nullptr) { + const std::string error_string{proj_errno_string(proj_context_errno(context))}; + throw std::invalid_argument("Could not create PROJ transform: " + error_string + '.'); + } + + auto coord_wgs84 = proj_coord( + carma_cooperative_perception::remove_units(coordinate.latitude), + carma_cooperative_perception::remove_units(coordinate.longitude), 0, 0); + auto coord_utm = proj_trans(utm_transformation, PJ_FWD, coord_wgs84); + + proj_destroy(utm_transformation); + proj_context_destroy(context); + + return { + utm_zone, units::length::meter_t{coord_utm.enu.e}, units::length::meter_t{coord_utm.enu.n}, + units::length::meter_t{coordinate.elevation}}; +} + +auto calculate_grid_convergence(const Wgs84Coordinate & position, const UtmZone & zone) + -> units::angle::degree_t +{ + gsl::owner context = proj_context_create(); + proj_log_level(context, PJ_LOG_NONE); + + if (context == nullptr) { + const std::string error_string{proj_errno_string(proj_context_errno(context))}; + throw std::invalid_argument("Could not create PROJ context: " + error_string + '.'); + } + + // N.B. developers: PROJ and the related geodetic calculations seem particularly sensitive + // to the parameters in this PROJ string. If you run into problems with you calculation + // results, carefully check this or any other PROJ string. + std::string proj_string{ + "+proj=utm +zone=" + std::to_string(zone.number) + " +datum=WGS84 +units=m +no_defs"}; + if (zone.hemisphere == Hemisphere::kSouth) { + proj_string += " +south"; + } + + gsl::owner transform = proj_create(context, proj_string.c_str()); + + const auto factors = proj_factors( + transform, proj_coord( + proj_torad(carma_cooperative_perception::remove_units(position.longitude)), + proj_torad(carma_cooperative_perception::remove_units(position.latitude)), 0, 0)); + + if (proj_context_errno(context) != 0) { + const std::string error_string{proj_errno_string(proj_context_errno(context))}; + throw std::invalid_argument("Could not calculate PROJ factors: " + error_string + '.'); + } + + proj_destroy(transform); + proj_context_destroy(context); + + return units::angle::degree_t{proj_todeg(factors.meridian_convergence)}; +} + +} // namespace carma_cooperative_perception diff --git a/carma_cooperative_perception/src/host_vehicle_filter_component.cpp b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp new file mode 100644 index 0000000000..562d7937b2 --- /dev/null +++ b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp @@ -0,0 +1,199 @@ +// Copyright 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 "carma_cooperative_perception/host_vehicle_filter_component.hpp" + +#include +#include +#include + +#include + +namespace carma_cooperative_perception +{ +auto HostVehicleFilterNode::handle_on_configure( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: configuring"); + + detection_list_sub_ = create_subscription< + carma_cooperative_perception_interfaces::msg::DetectionList>( + "input/detection_list", 1, + [this](const carma_cooperative_perception_interfaces::msg::DetectionList::SharedPtr msg_ptr) { + if (const auto current_state{this->get_current_state().label()}; current_state == "active") { + attempt_filter_and_republish(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->detection_list_sub_->get_topic_name(), current_state.c_str()); + } + }); + + host_vehicle_pose_sub_ = create_subscription( + "input/host_vehicle_pose", 1, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg_ptr) { + if (const auto current_state{this->get_current_state().label()}; current_state == "active") { + update_host_vehicle_pose(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->detection_list_sub_->get_topic_name(), current_state.c_str()); + } + }); + + detection_list_pub_ = + create_publisher( + "output/detection_list", 1); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured"); + + declare_parameter("distance_threshold_meters", 0.0); + + on_set_parameters_callback_ = + add_on_set_parameters_callback([this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "distance_threshold_meters") { + if (this->get_current_state().label() == "active") { + result.successful = false; + result.reason = "parameter is read-only while node is in 'Active' state"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'distance_threshold_meters': " + result.reason); + + break; + } + + if (const auto value{parameter.as_double()}; value < 0) { + result.successful = false; + result.reason = "parameter must be nonnegative"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'distance_threshold_meters': " + result.reason); + + break; + } else { + this->squared_distance_threshold_meters_ = std::pow(value, 2); + } + } else { + result.successful = false; + result.reason = "Unexpected parameter name '" + parameter.get_name() + '\''; + break; + } + } + + return result; + }); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: activating"); + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_deactivate( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating"); + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up"); + + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + detection_list_sub_.reset(); + host_vehicle_pose_sub_.reset(); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down"); + + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + detection_list_sub_.reset(); + host_vehicle_pose_sub_.reset(); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::update_host_vehicle_pose( + const geometry_msgs::msg::PoseStamped & msg) noexcept -> void +{ + host_vehicle_pose_ = msg; +} + +auto HostVehicleFilterNode::attempt_filter_and_republish( + carma_cooperative_perception_interfaces::msg::DetectionList msg) noexcept -> void +{ + if (!host_vehicle_pose_.has_value()) { + RCLCPP_WARN(get_logger(), "Could not filter detection list: host vehicle pose unknown"); + return; + } + + const auto is_within_distance = [this](const auto & detection) { + return euclidean_distance_squared(host_vehicle_pose_.value().pose, detection.pose.pose) <= + this->squared_distance_threshold_meters_; + }; + + const auto new_end{ + std::remove_if(std::begin(msg.detections), std::end(msg.detections), is_within_distance)}; + + msg.detections.erase(new_end, std::end(msg.detections)); + + this->detection_list_pub_->publish(msg); +} + +auto euclidean_distance_squared( + const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b) noexcept -> double +{ + return std::pow(a.position.x - b.position.x, 2) + std::pow(a.position.y - b.position.y, 2) + + std::pow(a.position.z - b.position.z, 2) + std::pow(a.orientation.x - b.orientation.x, 2) + + std::pow(a.orientation.y - b.orientation.y, 2) + + std::pow(a.orientation.z - b.orientation.z, 2) + + std::pow(a.orientation.w - b.orientation.w, 2); +} + +} // namespace carma_cooperative_perception + + +// This is not our macro, so we should not worry about linting it. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. +RCLCPP_COMPONENTS_REGISTER_NODE( // NOLINT + carma_cooperative_perception::HostVehicleFilterNode) // NOLINT \ No newline at end of file diff --git a/carma_cooperative_perception/src/host_vehicle_filter_node.cpp b/carma_cooperative_perception/src/host_vehicle_filter_node.cpp new file mode 100644 index 0000000000..529c485f01 --- /dev/null +++ b/carma_cooperative_perception/src/host_vehicle_filter_node.cpp @@ -0,0 +1,35 @@ +// Copyright 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 + +#include + +#include + +auto main(int argc, char * argv[]) noexcept -> int +{ + rclcpp::init(argc, argv); + + auto node{ + std::make_shared(rclcpp::NodeOptions{})}; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/carma_cooperative_perception/src/j2735_types.cpp b/carma_cooperative_perception/src/j2735_types.cpp new file mode 100644 index 0000000000..b605e33088 --- /dev/null +++ b/carma_cooperative_perception/src/j2735_types.cpp @@ -0,0 +1,121 @@ +// Copyright 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 "carma_cooperative_perception/j2735_types.hpp" + +namespace carma_cooperative_perception +{ +auto DDateTime::from_msg(const j2735_v2x_msgs::msg::DDateTime & msg) noexcept -> DDateTime +{ + DDateTime d_date_time; + + if (msg.presence_vector & msg.YEAR) { + d_date_time.year = units::time::year_t{static_cast(msg.year.year)}; + } + + if (msg.presence_vector & msg.MONTH) { + d_date_time.month = Month{msg.month.month}; + } + + if (msg.presence_vector & msg.DAY) { + d_date_time.day = units::time::day_t{static_cast(msg.day.day)}; + } + + if (msg.presence_vector & msg.HOUR) { + d_date_time.hour = units::time::hour_t{static_cast(msg.hour.hour)}; + } + + if (msg.presence_vector & msg.MINUTE) { + d_date_time.minute = units::time::minute_t{static_cast(msg.minute.minute)}; + } + + if (msg.presence_vector & msg.SECOND) { + d_date_time.second = units::time::millisecond_t{static_cast(msg.second.millisecond)}; + } + + if (msg.presence_vector & msg.OFFSET) { + d_date_time.time_zone_offset = + units::time::minute_t{static_cast(msg.offset.offset_minute)}; + } + + return d_date_time; +} + +auto AccelerationSet4Way::from_msg(const j2735_v2x_msgs::msg::AccelerationSet4Way & msg) noexcept + -> AccelerationSet4Way +{ + return { + units::acceleration::centi_meters_per_second_squared_t{static_cast(msg.longitudinal)}, + units::acceleration::centi_meters_per_second_squared_t{static_cast(msg.lateral)}, + units::acceleration::two_centi_standard_gravities_t{static_cast(msg.vert)}, + units::angular_velocity::centi_degrees_per_second_t{static_cast(msg.yaw_rate)}}; +} + +auto AccelerationSet4Way::from_msg(const carma_v2x_msgs::msg::AccelerationSet4Way & msg) noexcept + -> AccelerationSet4Way +{ + return { + units::acceleration::meters_per_second_squared_t{static_cast(msg.longitudinal)}, + units::acceleration::meters_per_second_squared_t{static_cast(msg.lateral)}, + units::acceleration::meters_per_second_squared_t{static_cast(msg.vert)}, + units::angular_velocity::degrees_per_second_t{static_cast(msg.yaw_rate)}}; +} + +auto Position3D::from_msg(const j2735_v2x_msgs::msg::Position3D & msg) noexcept -> Position3D +{ + Position3D position{ + units::angle::deci_micro_degrees_t{static_cast(msg.latitude)}, + units::angle::deci_micro_degrees_t{static_cast(msg.longitude)}, std::nullopt}; + + if (msg.elevation_exists) { + position.elevation = units::length::deca_centimeters_t{static_cast(msg.elevation)}; + } + + return position; +} + +auto Position3D::from_msg(const carma_v2x_msgs::msg::Position3D & msg) noexcept -> Position3D +{ + Position3D position{ + units::angle::degree_t{static_cast(msg.latitude)}, + units::angle::degree_t{static_cast(msg.longitude)}, std::nullopt}; + + if (msg.elevation_exists) { + position.elevation = units::length::meter_t{static_cast(msg.elevation)}; + } + + return position; +} + +auto Heading::from_msg(const j2735_v2x_msgs::msg::Heading & heading) noexcept -> Heading +{ + return {units::angle::eighth_deci_degrees_t{static_cast(heading.heading)}}; +} + +auto Heading::from_msg(const carma_v2x_msgs::msg::Heading & heading) noexcept -> Heading +{ + return {units::angle::degree_t{static_cast(heading.heading)}}; +} + +auto Speed::from_msg(const j2735_v2x_msgs::msg::Speed & speed) noexcept -> Speed +{ + return {units::velocity::two_centi_meters_per_second_t{static_cast(speed.speed)}}; +} + +auto Speed::from_msg(const carma_v2x_msgs::msg::Speed & speed) noexcept -> Speed +{ + return {units::velocity::meters_per_second_t{static_cast(speed.speed)}}; +} + +} // namespace carma_cooperative_perception diff --git a/carma_cooperative_perception/src/j3224_types.cpp b/carma_cooperative_perception/src/j3224_types.cpp new file mode 100644 index 0000000000..e95a43ae81 --- /dev/null +++ b/carma_cooperative_perception/src/j3224_types.cpp @@ -0,0 +1,59 @@ +// Copyright 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 "carma_cooperative_perception/j3224_types.hpp" + +namespace carma_cooperative_perception +{ +auto PositionOffsetXYZ::from_msg(const j3224_v2x_msgs::msg::PositionOffsetXYZ & msg) noexcept + -> PositionOffsetXYZ +{ + PositionOffsetXYZ offset{ + units::length::decimeter_t{static_cast(msg.offset_x.object_distance)}, + units::length::decimeter_t{static_cast(msg.offset_y.object_distance)}, std::nullopt}; + + if (msg.presence_vector & msg.HAS_OFFSET_Z) { + offset.offset_z = units::length::decimeter_t{static_cast(msg.offset_z.object_distance)}; + } + + return offset; +} + +auto PositionOffsetXYZ::from_msg(const carma_v2x_msgs::msg::PositionOffsetXYZ & msg) noexcept + -> PositionOffsetXYZ +{ + PositionOffsetXYZ offset{ + units::length::meter_t{static_cast(msg.offset_x.object_distance)}, + units::length::meter_t{static_cast(msg.offset_y.object_distance)}, std::nullopt}; + + if (msg.presence_vector & msg.HAS_OFFSET_Z) { + offset.offset_z = units::length::meter_t{static_cast(msg.offset_z.object_distance)}; + } + + return offset; +} + +auto MeasurementTimeOffset::from_msg( + const j3224_v2x_msgs::msg::MeasurementTimeOffset & msg) noexcept -> MeasurementTimeOffset +{ + return {units::time::millisecond_t{static_cast(msg.measurement_time_offset)}}; +} + +auto MeasurementTimeOffset::from_msg( + const carma_v2x_msgs::msg::MeasurementTimeOffset & msg) noexcept -> MeasurementTimeOffset +{ + return {units::time::second_t{static_cast(msg.measurement_time_offset)}}; +} + +} // namespace carma_cooperative_perception diff --git a/carma_cooperative_perception/src/msg_conversion.cpp b/carma_cooperative_perception/src/msg_conversion.cpp new file mode 100644 index 0000000000..a669aab47c --- /dev/null +++ b/carma_cooperative_perception/src/msg_conversion.cpp @@ -0,0 +1,574 @@ +// Copyright 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 "carma_cooperative_perception/msg_conversion.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "carma_cooperative_perception/geodetic.hpp" +#include "carma_cooperative_perception/j2735_types.hpp" +#include "carma_cooperative_perception/j3224_types.hpp" +#include "carma_cooperative_perception/units_extensions.hpp" + +#include +#include +#include +#include +#include "boost/date_time/posix_time/posix_time.hpp" + +namespace carma_cooperative_perception +{ +auto to_time_msg(const DDateTime & d_date_time) noexcept -> builtin_interfaces::msg::Time +{ + double seconds; + const auto fractional_secs{ + std::modf(remove_units(d_date_time.second.value_or(units::time::second_t{0.0})), &seconds)}; + + builtin_interfaces::msg::Time msg; + msg.sec = static_cast(seconds); + msg.nanosec = static_cast(fractional_secs * 1e9); + + return msg; +} + +auto calc_detection_time_stamp(DDateTime sdsm_time, const MeasurementTimeOffset & offset) noexcept + -> DDateTime +{ + sdsm_time.second.value() += offset.measurement_time_offset; + + return sdsm_time; +} + +auto to_ddate_time_msg(const builtin_interfaces::msg::Time & builtin_time) noexcept + -> j2735_v2x_msgs::msg::DDateTime +{ + j2735_v2x_msgs::msg::DDateTime ddate_time_output; + + // Add the time components from epoch seconds + boost::posix_time::ptime posix_time = boost::posix_time::from_time_t(builtin_time.sec) + + boost::posix_time::nanosec(builtin_time.nanosec); + + const auto time_stamp_year = posix_time.date().year(); + const auto time_stamp_month = posix_time.date().month(); + const auto time_stamp_day = posix_time.date().day(); + + const auto hours_of_day = posix_time.time_of_day().hours(); + const auto minutes_of_hour = posix_time.time_of_day().minutes(); + const auto seconds_of_minute = posix_time.time_of_day().seconds(); + + ddate_time_output.presence_vector = 0; + + ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::YEAR; + ddate_time_output.year.year = time_stamp_year; + ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::MONTH; + ddate_time_output.month.month = time_stamp_month; + ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::DAY; + ddate_time_output.day.day = time_stamp_day; + ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::HOUR; + ddate_time_output.hour.hour = hours_of_day; + ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::MINUTE; + ddate_time_output.minute.minute = minutes_of_hour; + ddate_time_output.presence_vector |= j2735_v2x_msgs::msg::DDateTime::SECOND; + ddate_time_output.second.millisecond = seconds_of_minute; + + return ddate_time_output; +} + +auto calc_sdsm_time_offset( + const builtin_interfaces::msg::Time & external_object_list_stamp, + const builtin_interfaces::msg::Time & external_object_stamp) noexcept + -> carma_v2x_msgs::msg::MeasurementTimeOffset +{ + carma_v2x_msgs::msg::MeasurementTimeOffset time_offset; + + boost::posix_time::ptime external_object_list_time = + boost::posix_time::from_time_t(external_object_list_stamp.sec) + + boost::posix_time::nanosec(external_object_list_stamp.nanosec); + + boost::posix_time::ptime external_object_time = + boost::posix_time::from_time_t(external_object_stamp.sec) + + boost::posix_time::nanosec(external_object_stamp.nanosec); + + boost::posix_time::time_duration offset_duration = + (external_object_list_time - external_object_time); + + time_offset.measurement_time_offset = offset_duration.total_seconds(); + + return time_offset; +} + +auto to_position_msg(const UtmCoordinate & position_utm) noexcept -> geometry_msgs::msg::Point +{ + geometry_msgs::msg::Point msg; + + msg.x = remove_units(position_utm.easting); + msg.y = remove_units(position_utm.northing); + msg.z = remove_units(position_utm.elevation); + + return msg; +} + +auto heading_to_enu_yaw(const units::angle::degree_t & heading) noexcept -> units::angle::degree_t +{ + return units::angle::degree_t{std::fmod(-(remove_units(heading) - 90.0) + 360.0, 360.0)}; +} + +auto enu_orientation_to_true_heading( + double yaw, const lanelet::BasicPoint3d & obj_pose, + const std::shared_ptr & map_projection) noexcept + -> units::angle::degree_t +{ + // Get object geodetic position + lanelet::GPSPoint wgs_obj_pose = map_projection->reverse(obj_pose); + + // Get WGS84 Heading + gsl::owner context = proj_context_create(); + gsl::owner transform = proj_create(context, map_projection->ECEF_PROJ_STR); + units::angle::degree_t grid_heading{std::fmod(90 - yaw + 360, 360)}; + + const auto factors = proj_factors( + transform, proj_coord(proj_torad(wgs_obj_pose.lon), proj_torad(wgs_obj_pose.lat), 0, 0)); + units::angle::degree_t grid_convergence{proj_todeg(factors.meridian_convergence)}; + + auto wgs_heading = grid_convergence + grid_heading; + + proj_destroy(transform); + proj_context_destroy(context); + + return wgs_heading; +} + +// determine the object position offset in m from the current reference pose +// in map frame and external object pose +auto calc_relative_position( + const geometry_msgs::msg::PoseStamped & source_pose, + const carma_v2x_msgs::msg::PositionOffsetXYZ & position_offset) noexcept + -> carma_v2x_msgs::msg::PositionOffsetXYZ +{ + carma_v2x_msgs::msg::PositionOffsetXYZ adjusted_offset; + + adjusted_offset.offset_x.object_distance = + position_offset.offset_x.object_distance - source_pose.pose.position.x; + adjusted_offset.offset_y.object_distance = + position_offset.offset_y.object_distance - source_pose.pose.position.y; + adjusted_offset.offset_z.object_distance = + position_offset.offset_z.object_distance - source_pose.pose.position.z; + adjusted_offset.presence_vector = carma_v2x_msgs::msg::PositionOffsetXYZ::HAS_OFFSET_Z; + + return adjusted_offset; +} + +auto transform_pose_from_map_to_wgs84( + const geometry_msgs::msg::PoseStamped & source_pose, + const std::shared_ptr & map_projection) noexcept + -> carma_v2x_msgs::msg::Position3D +{ + carma_v2x_msgs::msg::Position3D ref_pos; + lanelet::BasicPoint3d source_pose_basicpoint{ + source_pose.pose.position.x, source_pose.pose.position.y, 0.0}; + + lanelet::GPSPoint wgs84_ref_pose = map_projection->reverse(source_pose_basicpoint); + + ref_pos.longitude = wgs84_ref_pose.lon; + ref_pos.latitude = wgs84_ref_pose.lat; + ref_pos.elevation = wgs84_ref_pose.ele; + ref_pos.elevation_exists = true; + + return ref_pos; +} + +auto to_detection_list_msg(const carma_v2x_msgs::msg::SensorDataSharingMessage & sdsm) noexcept + -> carma_cooperative_perception_interfaces::msg::DetectionList +{ + carma_cooperative_perception_interfaces::msg::DetectionList detection_list; + + const auto ref_pos_3d{Position3D::from_msg(sdsm.ref_pos)}; + const Wgs84Coordinate ref_pos_wgs84{ + ref_pos_3d.latitude, ref_pos_3d.longitude, ref_pos_3d.elevation.value()}; + const auto ref_pos_utm{project_to_utm(ref_pos_wgs84)}; + + for (const auto & object_data : sdsm.objects.detected_object_data) { + const auto common_data{object_data.detected_object_common_data}; + + carma_cooperative_perception_interfaces::msg::Detection detection; + detection.header.frame_id = to_string(ref_pos_utm.utm_zone); + + const auto detection_time{calc_detection_time_stamp( + DDateTime::from_msg(sdsm.sdsm_time_stamp), + MeasurementTimeOffset::from_msg(common_data.measurement_time))}; + + detection.header.stamp = to_time_msg(detection_time); + + detection.id = std::to_string(common_data.detected_id.object_id); + + const auto pos_offset{PositionOffsetXYZ::from_msg(common_data.pos)}; + const auto utm_displacement{ + UtmDisplacement{pos_offset.offset_x, pos_offset.offset_y, pos_offset.offset_z.value()}}; + + const auto detection_pos_utm{ref_pos_utm + utm_displacement}; + detection.pose.pose.position = to_position_msg(detection_pos_utm); + + const auto true_heading{units::angle::degree_t{Heading::from_msg(common_data.heading).heading}}; + + // Note: This should really use the detection's WGS-84 position, so the + // convergence will be off slightly. TODO + const auto grid_convergence{calculate_grid_convergence(ref_pos_wgs84, ref_pos_utm.utm_zone)}; + + const auto grid_heading{true_heading - grid_convergence}; + const auto enu_yaw{heading_to_enu_yaw(grid_heading)}; + + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, remove_units(units::angle::radian_t{enu_yaw})); + detection.pose.pose.orientation = tf2::toMsg(quat_tf); + + const auto speed{Speed::from_msg(common_data.speed)}; + detection.twist.twist.linear.x = + remove_units(units::velocity::meters_per_second_t{speed.speed}); + + const auto speed_z{Speed::from_msg(common_data.speed_z)}; + detection.twist.twist.linear.z = + remove_units(units::velocity::meters_per_second_t{speed_z.speed}); + + const auto accel_set{AccelerationSet4Way::from_msg(common_data.accel_4_way)}; + detection.accel.accel.linear.x = + remove_units(units::acceleration::meters_per_second_squared_t{accel_set.longitudinal}); + detection.accel.accel.linear.y = + remove_units(units::acceleration::meters_per_second_squared_t{accel_set.lateral}); + detection.accel.accel.linear.z = + remove_units(units::acceleration::meters_per_second_squared_t{accel_set.vert}); + + detection.twist.twist.angular.z = + remove_units(units::angular_velocity::degrees_per_second_t{accel_set.yaw_rate}); + + switch (common_data.obj_type.object_type) { + case common_data.obj_type.ANIMAL: + detection.motion_model = detection.MOTION_MODEL_CTRV; + break; + case common_data.obj_type.VRU: + detection.motion_model = detection.MOTION_MODEL_CTRV; + break; + case common_data.obj_type.VEHICLE: + detection.motion_model = detection.MOTION_MODEL_CTRV; + break; + default: + detection.motion_model = detection.MOTION_MODEL_CTRV; + } + + detection_list.detections.push_back(std::move(detection)); + } + + return detection_list; +} + +auto to_detection_msg( + const carma_perception_msgs::msg::ExternalObject & object, + const MotionModelMapping & motion_model_mapping) noexcept + -> carma_cooperative_perception_interfaces::msg::Detection +{ + carma_cooperative_perception_interfaces::msg::Detection detection; + + detection.header = object.header; + + if (object.presence_vector & object.BSM_ID_PRESENCE_VECTOR) { + detection.id = ""; + std::transform( + std::cbegin(object.bsm_id), std::cend(object.bsm_id), std::back_inserter(detection.id), + [](const auto & i) { return i + '0'; }); + } + + if (object.presence_vector & object.ID_PRESENCE_VECTOR) { + detection.id += '-' + std::to_string(object.id); + } + + if (object.presence_vector & object.POSE_PRESENCE_VECTOR) { + detection.pose = object.pose; + } + + if (object.presence_vector & object.VELOCITY_INST_PRESENCE_VECTOR) { + detection.twist = object.velocity_inst; + } + + if (object.presence_vector & object.OBJECT_TYPE_PRESENCE_VECTOR) { + switch (object.object_type) { + case object.SMALL_VEHICLE: + detection.motion_model = motion_model_mapping.small_vehicle_model; + break; + case object.LARGE_VEHICLE: + detection.motion_model = motion_model_mapping.large_vehicle_model; + break; + case object.MOTORCYCLE: + detection.motion_model = motion_model_mapping.motorcycle_model; + break; + case object.PEDESTRIAN: + detection.motion_model = motion_model_mapping.pedestrian_model; + break; + case object.UNKNOWN: + default: + detection.motion_model = motion_model_mapping.unknown_model; + } + } + + return detection; +} + +auto to_detection_list_msg( + const carma_perception_msgs::msg::ExternalObjectList & object_list, + const MotionModelMapping & motion_model_mapping) noexcept + -> carma_cooperative_perception_interfaces::msg::DetectionList +{ + carma_cooperative_perception_interfaces::msg::DetectionList detection_list; + + std::transform( + std::cbegin(object_list.objects), std::cend(object_list.objects), + std::back_inserter(detection_list.detections), + [&motion_model_mapping = std::as_const(motion_model_mapping)](const auto & object) { + return to_detection_msg(object, motion_model_mapping); + }); + + return detection_list; +} + +auto to_external_object_msg( + const carma_cooperative_perception_interfaces::msg::Track & track) noexcept + -> carma_perception_msgs::msg::ExternalObject +{ + carma_perception_msgs::msg::ExternalObject external_object; + external_object.header = track.header; + external_object.presence_vector = 0; + + try { + if (const auto numeric_id{std::stol(track.id)}; + numeric_id >= 0 && numeric_id <= std::numeric_limits::max()) { + external_object.presence_vector |= external_object.ID_PRESENCE_VECTOR; + external_object.id = static_cast(numeric_id); + } + } catch (const std::invalid_argument & /* exception */) { + external_object.presence_vector &= ~external_object.ID_PRESENCE_VECTOR; + } catch (const std::out_of_range & /* exception */) { + external_object.presence_vector &= ~external_object.ID_PRESENCE_VECTOR; + } + + external_object.presence_vector |= external_object.POSE_PRESENCE_VECTOR; + external_object.pose = track.pose; + + external_object.presence_vector |= external_object.VELOCITY_PRESENCE_VECTOR; + external_object.velocity = track.twist; + + return external_object; +} + +auto to_external_object_list_msg( + const carma_cooperative_perception_interfaces::msg::TrackList & track_list) noexcept + -> carma_perception_msgs::msg::ExternalObjectList +{ + carma_perception_msgs::msg::ExternalObjectList external_object_list; + + for (const auto & track : track_list.tracks) { + external_object_list.objects.push_back(to_external_object_msg(track)); + } + + return external_object_list; +} + +auto to_sdsm_msg( + const carma_perception_msgs::msg::ExternalObjectList & external_object_list, + const geometry_msgs::msg::PoseStamped & current_pose, + const std::shared_ptr & map_projection) noexcept + -> carma_v2x_msgs::msg::SensorDataSharingMessage +{ + carma_v2x_msgs::msg::SensorDataSharingMessage sdsm; + carma_v2x_msgs::msg::DetectedObjectList detected_object_list; + + sdsm.sdsm_time_stamp = to_ddate_time_msg(external_object_list.header.stamp); + + sdsm.ref_pos = transform_pose_from_map_to_wgs84(current_pose, map_projection); + + // Convert external objects within the list to detected_object_data + for (const auto & external_object : external_object_list.objects) { + auto sdsm_detected_object = to_detected_object_data_msg(external_object, map_projection); + + // Calculate the time offset between individual objects and the respective SDSM container msg + sdsm_detected_object.detected_object_common_data.measurement_time = + calc_sdsm_time_offset(external_object.header.stamp, external_object.header.stamp); + + // Calculate the position offset from the current reference pose (in m) + sdsm_detected_object.detected_object_common_data.pos = + calc_relative_position(current_pose, sdsm_detected_object.detected_object_common_data.pos); + + detected_object_list.detected_object_data.push_back(sdsm_detected_object); + } + sdsm.objects = detected_object_list; + + return sdsm; +} + +auto to_detected_object_data_msg( + const carma_perception_msgs::msg::ExternalObject & external_object, + const std::shared_ptr & map_projection) noexcept + -> carma_v2x_msgs::msg::DetectedObjectData +{ + carma_v2x_msgs::msg::DetectedObjectData detected_object_data; + detected_object_data.presence_vector = 0; + + carma_v2x_msgs::msg::DetectedObjectCommonData detected_object_common_data; + detected_object_common_data.presence_vector = 0; + + // common data ////////// + + // obj_type_conf - convert from percentile, cast to proper uint type + if (external_object.presence_vector & external_object.OBJECT_TYPE_PRESENCE_VECTOR) { + detected_object_common_data.obj_type_cfd.classification_confidence = + static_cast(external_object.confidence * 100); + } + + // detected_id - cast proper type + if (external_object.presence_vector & external_object.ID_PRESENCE_VECTOR) { + detected_object_common_data.detected_id.object_id = + static_cast(external_object.id); + } + + // pos - Add offset to ref_pos to get object position + // in map frame -> convert to WGS84 coordinates for sdsm + + // To get offset: Subtract the external object pose from + // the current vehicle location given by the current_pose topic + if (external_object.presence_vector & external_object.POSE_PRESENCE_VECTOR) { + detected_object_common_data.pos.offset_x.object_distance = + static_cast(external_object.pose.pose.position.x); + detected_object_common_data.pos.offset_y.object_distance = + static_cast(external_object.pose.pose.position.y); + detected_object_common_data.pos.offset_z.object_distance = + static_cast(external_object.pose.pose.position.z); + } + + // speed/speed_z - convert vector velocity to scalar speed val given x/y components + if (external_object.presence_vector & external_object.VELOCITY_INST_PRESENCE_VECTOR) { + detected_object_common_data.speed.speed = std::hypot( + external_object.velocity_inst.twist.linear.x, external_object.velocity_inst.twist.linear.y); + + detected_object_common_data.presence_vector |= + carma_v2x_msgs::msg::DetectedObjectCommonData::HAS_SPEED_Z; + detected_object_common_data.speed_z.speed = external_object.velocity_inst.twist.linear.z; + + // heading - convert ang vel to scale heading + lanelet::BasicPoint3d external_object_position{ + external_object.pose.pose.position.x, external_object.pose.pose.position.y, + external_object.pose.pose.position.z}; + // Get yaw from orientation + auto obj_orientation = external_object.pose.pose.orientation; + tf2::Quaternion q(obj_orientation.x, obj_orientation.y, obj_orientation.z, obj_orientation.w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + detected_object_common_data.heading.heading = + remove_units(enu_orientation_to_true_heading(yaw, external_object_position, map_projection)); + } + + // optional data (determine based on object type) + // use object type struct for better control + carma_v2x_msgs::msg::DetectedObjectOptionalData detected_object_optional_data; + + switch (external_object.object_type) { + case external_object.SMALL_VEHICLE: + detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE; + + if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) { + detected_object_optional_data.det_veh.presence_vector = + carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE; + detected_object_optional_data.det_veh.presence_vector |= + carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT; + + detected_object_optional_data.det_veh.size.vehicle_width = external_object.size.y; + detected_object_optional_data.det_veh.size.vehicle_length = external_object.size.x; + detected_object_optional_data.det_veh.height.vehicle_height = external_object.size.z; + } + break; + case external_object.LARGE_VEHICLE: + detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE; + + if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) { + detected_object_optional_data.det_veh.presence_vector = + carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE; + detected_object_optional_data.det_veh.presence_vector |= + carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT; + + detected_object_optional_data.det_veh.size.vehicle_width = external_object.size.y; + detected_object_optional_data.det_veh.size.vehicle_length = external_object.size.x; + detected_object_optional_data.det_veh.height.vehicle_height = external_object.size.z; + } + break; + case external_object.MOTORCYCLE: + detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VEHICLE; + + if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) { + detected_object_optional_data.det_veh.presence_vector = + carma_v2x_msgs::msg::DetectedVehicleData::HAS_SIZE; + detected_object_optional_data.det_veh.presence_vector |= + carma_v2x_msgs::msg::DetectedVehicleData::HAS_HEIGHT; + + detected_object_optional_data.det_veh.size.vehicle_width = external_object.size.y; + detected_object_optional_data.det_veh.size.vehicle_length = external_object.size.x; + detected_object_optional_data.det_veh.height.vehicle_height = external_object.size.z; + } + break; + case external_object.PEDESTRIAN: + detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::VRU; + + detected_object_optional_data.det_vru.presence_vector = + carma_v2x_msgs::msg::DetectedVRUData::HAS_BASIC_TYPE; + detected_object_optional_data.det_vru.basic_type.type |= + j2735_v2x_msgs::msg::PersonalDeviceUserType::A_PEDESTRIAN; + + break; + case external_object.UNKNOWN: + default: + detected_object_common_data.obj_type.object_type = j3224_v2x_msgs::msg::ObjectType::UNKNOWN; + + if (external_object.presence_vector & external_object.SIZE_PRESENCE_VECTOR) { + detected_object_optional_data.det_obst.obst_size.width.size_value = external_object.size.y; + detected_object_optional_data.det_obst.obst_size.length.size_value = external_object.size.x; + + detected_object_optional_data.det_obst.obst_size.presence_vector = + carma_v2x_msgs::msg::ObstacleSize::HAS_HEIGHT; + detected_object_optional_data.det_obst.obst_size.height.size_value = external_object.size.z; + } + } + + detected_object_data.detected_object_common_data = std::move(detected_object_common_data); + detected_object_data.detected_object_optional_data = std::move(detected_object_optional_data); + + return detected_object_data; +} + +} // namespace carma_cooperative_perception diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp new file mode 100644 index 0000000000..e083ee8f87 --- /dev/null +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -0,0 +1,483 @@ +// Copyright 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 "carma_cooperative_perception/multiple_object_tracker_component.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace carma_cooperative_perception +{ + +namespace mot = cooperative_perception; + +auto make_ctrv_detection( + const carma_cooperative_perception_interfaces::msg::Detection & msg) noexcept -> Detection +{ + const auto timestamp{ + units::time::second_t{static_cast(msg.header.stamp.sec)} + + units::time::nanosecond_t{static_cast(msg.header.stamp.nanosec)}}; + + tf2::Quaternion orientation; + orientation.setX(msg.pose.pose.orientation.x); + orientation.setY(msg.pose.pose.orientation.y); + orientation.setZ(msg.pose.pose.orientation.z); + orientation.setW(msg.pose.pose.orientation.w); + + double roll{0.0}; + double pitch{0.0}; + double yaw{0.0}; + + tf2::Matrix3x3 matrix{orientation}; + matrix.getRPY(roll, pitch, yaw); + + const mot::CtrvState state{ + units::length::meter_t{msg.pose.pose.position.x}, + units::length::meter_t{msg.pose.pose.position.y}, + units::velocity::meters_per_second_t{msg.twist.twist.linear.x}, + mot::Angle{units::angle::radian_t{yaw}}, + units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z}}; + + const mot::CtrvStateCovariance covariance = mot::CtrvStateCovariance::Zero(); + + return mot::CtrvDetection{timestamp, state, covariance, mot::Uuid{msg.id}}; +} + +auto make_ctra_detection( + const carma_cooperative_perception_interfaces::msg::Detection & msg) noexcept -> Detection +{ + const auto timestamp{ + units::time::second_t{static_cast(msg.header.stamp.sec)} + + units::time::nanosecond_t{static_cast(msg.header.stamp.nanosec)}}; + + tf2::Quaternion orientation; + orientation.setX(msg.pose.pose.orientation.x); + orientation.setY(msg.pose.pose.orientation.y); + orientation.setZ(msg.pose.pose.orientation.z); + orientation.setW(msg.pose.pose.orientation.w); + + double roll{0.0}; + double pitch{0.0}; + double yaw{0.0}; + + tf2::Matrix3x3 matrix{orientation}; + matrix.getRPY(roll, pitch, yaw); + + const mot::CtraState state{ + units::length::meter_t{msg.pose.pose.position.x}, + units::length::meter_t{msg.pose.pose.position.y}, + units::velocity::meters_per_second_t{msg.twist.twist.linear.x}, + mot::Angle{units::angle::radian_t{yaw}}, + units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z}, + units::acceleration::meters_per_second_squared_t{msg.accel.accel.linear.x}}; + + const mot::CtraStateCovariance covariance = mot::CtraStateCovariance::Zero(); + + return mot::CtraDetection{timestamp, state, covariance, mot::Uuid{msg.id}}; +} + +auto make_detection(const carma_cooperative_perception_interfaces::msg::Detection & msg) + -> Detection +{ + switch (msg.motion_model) { + case msg.MOTION_MODEL_CTRV: + return make_ctrv_detection(msg); + + case msg.MOTION_MODEL_CTRA: + return make_ctra_detection(msg); + + case msg.MOTION_MODEL_CV: + break; + } + + throw std::runtime_error("unkown motion model type '" + std::to_string(msg.motion_model) + "'"); +} + +static auto to_ros_msg(const mot::CtraTrack & track) noexcept +{ + carma_cooperative_perception_interfaces::msg::Track msg; + + msg.header.stamp.sec = mot::remove_units(units::math::floor(track.timestamp)); + msg.header.stamp.nanosec = mot::remove_units( + units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{10.0})}); + msg.header.frame_id = "map"; + + msg.id = track.uuid.value(); + msg.motion_model = msg.MOTION_MODEL_CTRA; + msg.pose.pose.position.x = mot::remove_units(track.state.position_x); + msg.pose.pose.position.y = mot::remove_units(track.state.position_y); + + tf2::Quaternion orientation; + orientation.setRPY(0, 0, mot::remove_units(track.state.yaw.get_angle())); + msg.pose.pose.orientation.x = orientation.getX(); + msg.pose.pose.orientation.y = orientation.getY(); + msg.pose.pose.orientation.z = orientation.getZ(); + msg.pose.pose.orientation.w = orientation.getW(); + + msg.twist.twist.linear.x = mot::remove_units(track.state.velocity); + msg.twist.twist.angular.z = mot::remove_units(track.state.yaw_rate); + + msg.accel.accel.linear.x = mot::remove_units(track.state.acceleration); + + return msg; +} + +static auto to_ros_msg(const mot::CtrvTrack & track) noexcept +{ + carma_cooperative_perception_interfaces::msg::Track msg; + + msg.header.stamp.sec = mot::remove_units(units::math::floor(track.timestamp)); + msg.header.stamp.nanosec = mot::remove_units( + units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{10.0})}); + msg.header.frame_id = "map"; + + msg.id = track.uuid.value(); + msg.motion_model = msg.MOTION_MODEL_CTRV; + msg.pose.pose.position.x = mot::remove_units(track.state.position_x); + msg.pose.pose.position.y = mot::remove_units(track.state.position_y); + + tf2::Quaternion orientation; + orientation.setRPY(0, 0, mot::remove_units(track.state.yaw.get_angle())); + msg.pose.pose.orientation.x = orientation.getX(); + msg.pose.pose.orientation.y = orientation.getY(); + msg.pose.pose.orientation.z = orientation.getZ(); + msg.pose.pose.orientation.w = orientation.getW(); + + msg.twist.twist.linear.x = mot::remove_units(track.state.velocity); + msg.twist.twist.angular.z = mot::remove_units(track.state.yaw_rate); + + return msg; +} + +static auto to_ros_msg(const Track & track) +{ + static constexpr mot::Visitor visitor{ + [](const mot::CtrvTrack & t) { return to_ros_msg(t); }, + [](const mot::CtraTrack & t) { return to_ros_msg(t); }, + [](const auto &) { throw std::runtime_error{"cannot make ROS 2 message from track type"}; }}; + + return std::visit(visitor, track); +} + +MultipleObjectTrackerNode::MultipleObjectTrackerNode(const rclcpp::NodeOptions & options) +: CarmaLifecycleNode{options} +{ +} + +auto MultipleObjectTrackerNode::handle_on_configure( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: configuring"); + + track_list_pub_ = create_publisher( + "output/track_list", 1); + + detection_list_sub_ = create_subscription< + carma_cooperative_perception_interfaces::msg::DetectionList>( + "input/detection_list", 1, + [this](const carma_cooperative_perception_interfaces::msg::DetectionList::SharedPtr msg_ptr) { + if (const auto current_state{this->get_current_state().label()}; current_state == "active") { + this->store_new_detections(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->detection_list_sub_->get_topic_name(), current_state.c_str()); + } + }); + + declare_parameter( + "execution_frequency_hz", mot::remove_units(units::frequency::hertz_t{1 / execution_period_})); + + declare_parameter( + "track_promotion_threshold", static_cast(track_manager_.get_promotion_threshold().value)); + + declare_parameter( + "track_removal_threshold", static_cast(track_manager_.get_promotion_threshold().value)); + + on_set_parameters_callback_ = + add_on_set_parameters_callback([this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "execution_frequency_hz") { + if (this->get_current_state().label() == "active") { + result.successful = false; + result.reason = "parameter is read-only while node is in 'Active' state"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason); + + break; + } else { + this->execution_period_ = 1 / units::frequency::hertz_t{parameter.as_double()}; + } + } else if (parameter.get_name() == "track_promotion_threshold") { + if (this->get_current_state().label() == "active") { + result.successful = false; + result.reason = "parameter is read-only while node is in 'Active' state"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason); + + break; + } else { + if (const auto value{parameter.as_int()}; value < 0) { + result.successful = false; + result.reason = "parameter must be nonnegative"; + } else { + this->track_manager_.set_promotion_threshold_and_update( + mot::PromotionThreshold{static_cast(value)}); + } + } + } else if (parameter.get_name() == "track_removal_threshold") { + if (this->get_current_state().label() == "active") { + result.successful = false; + result.reason = "parameter is read-only while node is in 'Active' state"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason); + + break; + } else { + if (const auto value{parameter.as_int()}; value < 0) { + result.successful = false; + result.reason = "parameter must be nonnegative"; + } else { + this->track_manager_.set_removal_threshold_and_update( + mot::RemovalThreshold{static_cast(value)}); + } + } + } else { + result.successful = false; + result.reason = "Unexpected parameter name '" + parameter.get_name() + '\''; + } + } + + return result; + }); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto MultipleObjectTrackerNode::handle_on_activate( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: activating"); + + if (pipeline_execution_timer_ != nullptr) { + // The user might have changed the timer period since last time the Node was active + pipeline_execution_timer_->reset(); + } + + const std::chrono::duration period_ns{mot::remove_units(execution_period_)}; + pipeline_execution_timer_ = create_wall_timer(period_ns, [this] { execute_pipeline(); }); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto MultipleObjectTrackerNode::handle_on_deactivate( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating"); + + // There is currently no way to change a timer's period in ROS 2, so we will + // have to create a new one in case a user changes the period. + pipeline_execution_timer_.reset(); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto MultipleObjectTrackerNode::handle_on_cleanup( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up"); + + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + detection_list_sub_.reset(); + + undeclare_parameter("execution_frequency_hz"); + remove_on_set_parameters_callback(on_set_parameters_callback_.get()); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto MultipleObjectTrackerNode::handle_on_shutdown( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down"); + + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + detection_list_sub_.reset(); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto MultipleObjectTrackerNode::store_new_detections( + const carma_cooperative_perception_interfaces::msg::DetectionList & msg) noexcept -> void +{ + if (std::size(msg.detections) == 0) { + RCLCPP_WARN(this->get_logger(), "Not storing detections: incoming detection list is empty"); + return; + } + + for (const auto & detection_msg : msg.detections) { + try { + const auto detection{make_detection(detection_msg)}; + const auto uuid{mot::get_uuid(detection)}; + + if (uuid_index_map_.count(uuid) == 0) { + detections_.push_back(std::move(detection)); + uuid_index_map_[uuid] = std::size(detections_) - 1; + } else { + RCLCPP_WARN_STREAM( + this->get_logger(), + "Detection with ID '" << uuid << "' already exists. Overwriting its data"); + detections_.at(uuid_index_map_[uuid]) = detection; + } + } catch (const std::runtime_error & error) { + RCLCPP_ERROR( + this->get_logger(), "Ignoring detection with ID '%s': %s", detection_msg.id.c_str(), + error.what()); + } + } +} + +static auto temporally_align_detections( + std::vector & detections, units::time::second_t end_time) noexcept -> void +{ + for (auto & detection : detections) { + mot::propagate_to_time(detection, end_time, mot::default_unscented_transform); + } +} + +static auto predict_track_states(std::vector tracks, units::time::second_t end_time) noexcept +{ + for (auto & track : tracks) { + mot::propagate_to_time(track, end_time, mot::default_unscented_transform); + } + + return tracks; +} + +auto MultipleObjectTrackerNode::execute_pipeline() -> void +{ + static constexpr mot::Visitor make_track_visitor{ + [](const mot::CtrvDetection & d) { return Track{mot::make_track(d)}; }, + [](const mot::CtraDetection & d) { return Track{mot::make_track(d)}; }, + [](const auto &) { throw std::runtime_error("cannot make track from given detection"); }, + }; + + if (track_manager_.get_all_tracks().empty()) { + RCLCPP_DEBUG( + get_logger(), "List of tracks is empty. Converting detections to tentative tracks"); + + const auto clusters{mot::cluster_detections(detections_, 0.75)}; + for (const auto & cluster : clusters) { + const auto detection{std::cbegin(cluster.get_detections())->second}; + track_manager_.add_tentative_track(std::visit(make_track_visitor, detection)); + } + + track_list_pub_->publish(carma_cooperative_perception_interfaces::msg::TrackList{}); + + detections_.clear(); + uuid_index_map_.clear(); + return; + } + + const units::time::second_t current_time{this->now().seconds()}; + + temporally_align_detections(detections_, current_time); + + const auto predicted_tracks{predict_track_states(track_manager_.get_all_tracks(), current_time)}; + const auto scores{ + mot::score_tracks_and_detections(predicted_tracks, detections_, mot::euclidean_distance_score)}; + + const auto associations{mot::associate_detections_to_tracks(scores, mot::gnn_associator)}; + track_manager_.update_track_lists(associations); + + std::unordered_map detection_map; + for (const auto & detection : detections_) { + detection_map[mot::get_uuid(detection)] = detection; + } + + const mot::HasAssociation has_association{associations}; + for (auto & track : track_manager_.get_all_tracks()) { + if (has_association(track)) { + const auto detection_uuids{associations.at(get_uuid(track))}; + const auto first_detection{detection_map[detection_uuids.at(0)]}; + track = std::visit(mot::covariance_intersection_visitor, track, first_detection); + } + } + + // Unassociated detections don't influence the tracking pipeline, so we can add + // them to the tracker at the end. + std::vector unassociated_detections; + for (const auto & [uuid, detection] : detection_map) { + if (!has_association(detection)) { + unassociated_detections.push_back(detection); + } + } + + const auto clusters{mot::cluster_detections(unassociated_detections, 0.75)}; + for (const auto & cluster : clusters) { + const auto detection{std::cbegin(cluster.get_detections())->second}; + track_manager_.add_tentative_track(std::visit(make_track_visitor, detection)); + } + + carma_cooperative_perception_interfaces::msg::TrackList track_list; + for (const auto & track : track_manager_.get_confirmed_tracks()) { + track_list.tracks.push_back(to_ros_msg(track)); + } + + track_list_pub_->publish(track_list); + + detections_.clear(); + uuid_index_map_.clear(); +} + +} // namespace carma_cooperative_perception + +// This is not our macro, so we should not worry about linting it. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. +RCLCPP_COMPONENTS_REGISTER_NODE(carma_cooperative_perception::MultipleObjectTrackerNode) // NOLINT diff --git a/carma_cooperative_perception/src/multiple_object_tracker_node.cpp b/carma_cooperative_perception/src/multiple_object_tracker_node.cpp new file mode 100644 index 0000000000..f4281e527a --- /dev/null +++ b/carma_cooperative_perception/src/multiple_object_tracker_node.cpp @@ -0,0 +1,35 @@ +// Copyright 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 + +#include + +#include + +auto main(int argc, char * argv[]) noexcept -> int +{ + rclcpp::init(argc, argv); + + auto node{std::make_shared( + rclcpp::NodeOptions{})}; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/carma_cooperative_perception/src/sdsm_to_detection_list_component.cpp b/carma_cooperative_perception/src/sdsm_to_detection_list_component.cpp new file mode 100644 index 0000000000..41c770692e --- /dev/null +++ b/carma_cooperative_perception/src/sdsm_to_detection_list_component.cpp @@ -0,0 +1,23 @@ +// Copyright 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 "carma_cooperative_perception/sdsm_to_detection_list_component.hpp" + +#include + +// This is not our macro, so we should not worry about linting it. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. +RCLCPP_COMPONENTS_REGISTER_NODE(carma_cooperative_perception::SdsmToDetectionListNode) // NOLINT diff --git a/carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp b/carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp new file mode 100644 index 0000000000..1e7d1f6409 --- /dev/null +++ b/carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp @@ -0,0 +1,93 @@ +// Copyright 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 "carma_cooperative_perception/track_list_to_external_object_list_component.hpp" + +#include +#include +#include +#include + +#include "carma_cooperative_perception/external_object_list_to_detection_list_component.hpp" +#include "carma_cooperative_perception/geodetic.hpp" +#include "carma_cooperative_perception/units_extensions.hpp" + +namespace carma_cooperative_perception +{ +TrackListToExternalObjectListNode::TrackListToExternalObjectListNode( + const rclcpp::NodeOptions & options) +: CarmaLifecycleNode{options} +{ + lifecycle_publishers_.push_back(publisher_); + param_callback_handles_.push_back(on_set_parameters_callback_); +} + +auto TrackListToExternalObjectListNode::handle_on_configure( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + publisher_ = create_publisher( + "output/external_object_list", 1); + + track_list_subscription_ = create_subscription< + carma_cooperative_perception_interfaces::msg::TrackList>( + "input/track_list", 1, + [this](const carma_cooperative_perception_interfaces::msg::TrackList::SharedPtr msg_ptr) { + if (const auto current_state{this->get_current_state().label()}; current_state == "active") { + publish_as_external_object_list(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->track_list_subscription_->get_topic_name(), current_state.c_str()); + } + }); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto TrackListToExternalObjectListNode::handle_on_cleanup( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + track_list_subscription_.reset(); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto TrackListToExternalObjectListNode::handle_on_shutdown( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + track_list_subscription_.reset(); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto TrackListToExternalObjectListNode::publish_as_external_object_list( + const carma_cooperative_perception_interfaces::msg::TrackList & msg) const noexcept -> void +{ + auto external_object_list{to_external_object_list_msg(msg)}; + external_object_list.header.stamp = now(); + + publisher_->publish(external_object_list); +} + +} // namespace carma_cooperative_perception + +// This is not our macro, so we should not worry about linting it. +// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes +// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but +// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0. +RCLCPP_COMPONENTS_REGISTER_NODE(carma_cooperative_perception::TrackListToExternalObjectListNode) // NOLINT \ No newline at end of file diff --git a/carma_cooperative_perception/src/track_list_to_external_object_list_node.cpp b/carma_cooperative_perception/src/track_list_to_external_object_list_node.cpp new file mode 100644 index 0000000000..b6a1633999 --- /dev/null +++ b/carma_cooperative_perception/src/track_list_to_external_object_list_node.cpp @@ -0,0 +1,35 @@ +// Copyright 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 + +#include + +#include + +auto main(int argc, char * argv[]) noexcept -> int +{ + rclcpp::init(argc, argv); + + auto node{std::make_shared( + rclcpp::NodeOptions{})}; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/carma_cooperative_perception/src/utm_zone.cpp b/carma_cooperative_perception/src/utm_zone.cpp new file mode 100644 index 0000000000..b162ecb53b --- /dev/null +++ b/carma_cooperative_perception/src/utm_zone.cpp @@ -0,0 +1,30 @@ +// Copyright 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 "carma_cooperative_perception/utm_zone.hpp" + +#include + +namespace carma_cooperative_perception +{ +auto to_string(const UtmZone & zone) -> std::string +{ + if (zone.hemisphere == Hemisphere::kNorth) { + return std::to_string(zone.number) + "N"; + } + + return std::to_string(zone.number) + "S"; +} + +} // namespace carma_cooperative_perception diff --git a/carma_cooperative_perception/test/data/detection_list.yaml b/carma_cooperative_perception/test/data/detection_list.yaml new file mode 100644 index 0000000000..ccdaa06020 --- /dev/null +++ b/carma_cooperative_perception/test/data/detection_list.yaml @@ -0,0 +1,19 @@ +--- +message_type: carma_cooperative_perception_interfaces/msg/DetectionList +message_fields: + detections: + - id: "1" + pose: + pose: + position: + x: 1.0 + - id: "2" + pose: + pose: + position: + x: 10.0 + - id: "3" + pose: + pose: + position: + x: 20.0 diff --git a/carma_cooperative_perception/test/data/detection_list_duplicates.yaml b/carma_cooperative_perception/test/data/detection_list_duplicates.yaml new file mode 100644 index 0000000000..473006b48a --- /dev/null +++ b/carma_cooperative_perception/test/data/detection_list_duplicates.yaml @@ -0,0 +1,51 @@ +--- +message_type: carma_cooperative_perception_interfaces/msg/DetectionList +message_fields: + detections: + - header: # obstacle detected from viewer A + stamp: + sec: 0 + nanosec: 0 + frame_id: "map" + id: "detection1" + motion_model: 1 # CTRV + pose: + pose: + position: + x: 5.001 # meters + y: 5.0 # meters + orientation: + x: 0 + y: 0 + z: 0 + w: 1 + twist: + twist: + linear: + x: 0.0 # meters per second + angular: + z: 0.0 # radians per second + + - header: # obstacle detected from viewer B + stamp: + sec: 0 + nanosec: 0 + frame_id: "map" + id: "detection2" + motion_model: 1 # CTRV + pose: + pose: + position: + x: 5.0 # meters + y: 5.0 # meters + orientation: + x: 0 + y: 0 + z: 0 + w: 1 + twist: + twist: + linear: + x: 0.0 # meters per second + angular: + z: 0.0 # radians per second diff --git a/carma_cooperative_perception/test/data/detection_list_static.yaml b/carma_cooperative_perception/test/data/detection_list_static.yaml new file mode 100644 index 0000000000..2469356347 --- /dev/null +++ b/carma_cooperative_perception/test/data/detection_list_static.yaml @@ -0,0 +1,51 @@ +--- +message_type: carma_cooperative_perception_interfaces/msg/DetectionList +message_fields: + detections: + - header: # obstacle detected from viewer A + stamp: + sec: 0 + nanosec: 0 + frame_id: "map" + id: "detection1" + motion_model: 1 # CTRV + pose: + pose: + position: + x: 5.0 # meters + y: 5.0 # meters + orientation: + x: 0 + y: 0 + z: 0 + w: 1 + twist: + twist: + linear: + x: 0.0 # meters per second + angular: + z: 0.0 # radians per second + + - header: # obstacle detected from viewer B + stamp: + sec: 0 + nanosec: 0 + frame_id: "map" + id: "detection2" + motion_model: 1 # CTRV + pose: + pose: + position: + x: 6.0 # meters + y: 5.0 # meters + orientation: + x: 0 + y: 0 + z: 0 + w: 1 + twist: + twist: + linear: + x: 0.0 # meters per second + angular: + z: 0.0 # radians per second diff --git a/carma_cooperative_perception/test/data/track_list.yaml b/carma_cooperative_perception/test/data/track_list.yaml new file mode 100644 index 0000000000..4729cf166d --- /dev/null +++ b/carma_cooperative_perception/test/data/track_list.yaml @@ -0,0 +1,7 @@ +--- +message_type: carma_cooperative_perception_interfaces/msg/TrackList +message_fields: + tracks: + - id: "1" + - id: "2" + - id: "3" diff --git a/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py b/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py new file mode 100644 index 0000000000..4dedf8d50c --- /dev/null +++ b/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py @@ -0,0 +1,137 @@ +# Copyright 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. + +import unittest + +from ament_index_python import get_package_share_path + +from carma_launch_testing.transitions import transition_node +from carma_launch_testing.predicates import LenIncreases +from carma_launch_testing.spinning import spin_node_until + +import carma_message_utilities + +from carma_cooperative_perception_interfaces.msg import DetectionList + +from geometry_msgs.msg import PoseStamped + +import launch_ros.actions + +from launch import LaunchDescription +from launch.actions import TimerAction + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +from rclpy.context import Context +import rclpy.node + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.detection_list_pub = self.create_publisher( + DetectionList, "input/detection_list", 1 + ) + + self.host_vehicle_pose_pub = self.create_publisher( + PoseStamped, "input/host_vehicle_pose", 1 + ) + + self.detection_list_sub = self.create_subscription( + DetectionList, + "output/detection_list", + lambda msg: self.detection_list_msgs.append(msg), + 1, + ) + + self.detection_list_msgs = [] + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = launch_ros.actions.Node( + package="carma_cooperative_perception", + executable="host_vehicle_filter_node", + name="node_under_test", + parameters=[{"distance_threshold_meters": 2.0}], + ) + + launch_description = LaunchDescription( + [node_under_test, TimerAction(period=1.0, actions=[ReadyToTest()])] + ) + + return launch_description + + +class TestMotionComputation(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_track_list_conversion(self): + transition_id = Transition.TRANSITION_CONFIGURE + transition_node("node_under_test", transition_id, self.context) + + transition_id = Transition.TRANSITION_ACTIVATE + transition_node("node_under_test", transition_id, self.context) + + host_vehicle_pose = PoseStamped() + host_vehicle_pose.pose.position.x = 1.0 + + self.test_harness_node.host_vehicle_pose_pub.publish(host_vehicle_pose) + + package_share_path = get_package_share_path("carma_cooperative_perception") + + msg_file = package_share_path / "test/data/detection_list.yaml" + msg = carma_message_utilities.msg_from_yaml_file(msg_file) + self.test_harness_node.detection_list_pub.publish(msg) + + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.detection_list_msgs), + self.context, + ) + + self.assertGreaterEqual(len(self.test_harness_node.detection_list_msgs), 0) + detection_list = self.test_harness_node.detection_list_msgs[-1] + + self.assertEqual(len(detection_list.detections), 2) + + self.assertEqual(detection_list.detections[0].id, '2') + self.assertEqual(detection_list.detections[1].id, '3') + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/carma_cooperative_perception/test/multiple_object_tracker_duplicates_launch_test.py b/carma_cooperative_perception/test/multiple_object_tracker_duplicates_launch_test.py new file mode 100644 index 0000000000..f563b883ed --- /dev/null +++ b/carma_cooperative_perception/test/multiple_object_tracker_duplicates_launch_test.py @@ -0,0 +1,159 @@ +# Copyright 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. + +from ament_index_python import get_package_share_path + +from carma_cooperative_perception_interfaces.msg import DetectionList, TrackList + +from carma_launch_testing.transitions import transition_node +from carma_launch_testing.predicates import LenIncreases +from carma_launch_testing.spinning import spin_node_until + +import carma_message_utilities + +from launch import LaunchDescription +from launch.actions import TimerAction + +import launch_ros.actions + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +from rclpy.context import Context +import rclpy.node + +import unittest + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.detection_list_list_pub = self.create_publisher( + DetectionList, "input/detection_list", 1 + ) + + self.track_list_sub = self.create_subscription( + TrackList, + "output/track_list", + self._store_track_list, + 1, + ) + + self.track_list_msgs = [] + + def _store_track_list(self, msg) -> None: + self.get_logger().info(f"Received track list message") + self.track_list_msgs.append(msg) + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = launch_ros.actions.Node( + package="carma_cooperative_perception", + executable="multiple_object_tracker_node", + name="node_under_test", + # arguments=["--ros-args", "--log-level", "debug"], + ) + + launch_description = LaunchDescription( + [node_under_test, TimerAction(period=1.0, actions=[ReadyToTest()])] + ) + + return launch_description + + +class TestMotionComputation(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_foo(self): + transition_id = Transition.TRANSITION_CONFIGURE + transition_node("node_under_test", transition_id, self.context) + + transition_id = Transition.TRANSITION_ACTIVATE + transition_node("node_under_test", transition_id, self.context) + + package_share_path = get_package_share_path("carma_cooperative_perception") + + msg_file = package_share_path / "test/data/detection_list_duplicates.yaml" + msg = carma_message_utilities.msg_from_yaml_file(msg_file) + + print("launch test: publishing message [1/3]") + self.test_harness_node.detection_list_list_pub.publish(msg) + + print("launch test: waiting on output from node_under_test") + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.track_list_msgs), + self.context, + ) + + latest_msg = self.test_harness_node.track_list_msgs[-1] + self.assertEqual(len(latest_msg.tracks), 0) + + print("launch test: publishing message [2/3]") + self.test_harness_node.detection_list_list_pub.publish(msg) + + print("launch test: waiting on output from node_under_test") + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.track_list_msgs), + self.context, + ) + + latest_msg = self.test_harness_node.track_list_msgs[-1] + self.assertEqual(len(latest_msg.tracks), 0) + + print("launch test: publishing message [3/3]") + self.test_harness_node.detection_list_list_pub.publish(msg) + + latest_msg = self.test_harness_node.track_list_msgs[-1] + self.assertEqual(len(latest_msg.tracks), 0) + + print("launch test: waiting on output from node_under_test") + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.track_list_msgs), + self.context, + ) + + self.assertGreater(len(self.test_harness_node.track_list_msgs), 0) + + latest_msg = self.test_harness_node.track_list_msgs[-1] + self.assertEqual(len(latest_msg.tracks), 1) + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/carma_cooperative_perception/test/multiple_object_tracker_static_obstacle_launch_test.py b/carma_cooperative_perception/test/multiple_object_tracker_static_obstacle_launch_test.py new file mode 100644 index 0000000000..96a49aa967 --- /dev/null +++ b/carma_cooperative_perception/test/multiple_object_tracker_static_obstacle_launch_test.py @@ -0,0 +1,150 @@ +# Copyright 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. + +from ament_index_python import get_package_share_path + +from carma_cooperative_perception_interfaces.msg import DetectionList, TrackList + +from carma_launch_testing.transitions import transition_node +from carma_launch_testing.predicates import LenIncreases +from carma_launch_testing.spinning import spin_node_until + +import carma_message_utilities + +from launch import LaunchDescription +from launch.actions import TimerAction + +import launch_ros.actions + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +from rclpy.context import Context +import rclpy.node + +import unittest + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.detection_list_list_pub = self.create_publisher( + DetectionList, "input/detection_list", 1 + ) + + self.track_list_sub = self.create_subscription( + TrackList, + "output/track_list", + self._store_track_list, + 1, + ) + + self.track_list_msgs = [] + + def _store_track_list(self, msg) -> None: + self.get_logger().info(f"Received track list message") + self.track_list_msgs.append(msg) + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = launch_ros.actions.Node( + package="carma_cooperative_perception", + executable="multiple_object_tracker_node", + name="node_under_test", + # arguments=["--ros-args", "--log-level", "debug"], + ) + + launch_description = LaunchDescription( + [node_under_test, TimerAction(period=1.0, actions=[ReadyToTest()])] + ) + + return launch_description + + +class TestMotionComputation(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_foo(self): + transition_id = Transition.TRANSITION_CONFIGURE + transition_node("node_under_test", transition_id, self.context) + + transition_id = Transition.TRANSITION_ACTIVATE + transition_node("node_under_test", transition_id, self.context) + + package_share_path = get_package_share_path("carma_cooperative_perception") + + msg_file = package_share_path / "test/data/detection_list_static.yaml" + msg = carma_message_utilities.msg_from_yaml_file(msg_file) + + print("launch test: publishing message [1/3]") + self.test_harness_node.detection_list_list_pub.publish(msg) + + print("launch test: waiting on output from node_under_test") + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.track_list_msgs), + self.context, + ) + + print("launch test: publishing message [2/3]") + self.test_harness_node.detection_list_list_pub.publish(msg) + + print("launch test: waiting on output from node_under_test") + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.track_list_msgs), + self.context, + ) + + print("launch test: publishing message [3/3]") + self.test_harness_node.detection_list_list_pub.publish(msg) + + print("launch test: waiting on output from node_under_test") + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.track_list_msgs), + self.context, + ) + + self.assertGreater(len(self.test_harness_node.track_list_msgs), 0) + + latest_msg = self.test_harness_node.track_list_msgs[-1] + self.assertEqual(len(latest_msg.tracks), 2) + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/carma_cooperative_perception/test/test_external_object_list_to_detection_list_component.cpp b/carma_cooperative_perception/test/test_external_object_list_to_detection_list_component.cpp new file mode 100644 index 0000000000..2a65aaf78c --- /dev/null +++ b/carma_cooperative_perception/test/test_external_object_list_to_detection_list_component.cpp @@ -0,0 +1,39 @@ +// Copyright 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 + +#include +#include +#include +#include + +TEST(TransformFromMapToUtm, Simple) +{ + carma_cooperative_perception_interfaces::msg::DetectionList detection_list; + carma_cooperative_perception_interfaces::msg::Detection detection; + detection_list.detections.push_back(std::move(detection)); + + // This PROJ string places the projection's origin over Washington, DC. + const std::string map_origin{ + "+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 " + "+datum=WGS84 +units=m +vunits=m +no_defs"}; + + const auto result{ + carma_cooperative_perception::transform_from_map_to_utm(detection_list, map_origin)}; + + EXPECT_EQ(result.detections.at(0).header.frame_id, "18N"); + EXPECT_NEAR(result.detections.at(0).pose.pose.position.x, 313835.60, 1e-1); + EXPECT_NEAR(result.detections.at(0).pose.pose.position.y, 4313642.41, 1e-1); +} diff --git a/carma_cooperative_perception/test/test_geodetic.cpp b/carma_cooperative_perception/test/test_geodetic.cpp new file mode 100644 index 0000000000..3cdd52eb2c --- /dev/null +++ b/carma_cooperative_perception/test/test_geodetic.cpp @@ -0,0 +1,135 @@ +// Copyright 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 + +#include +#include +#include + +TEST(CalculateUtmZone, Wgs84Coordinate) +{ + struct TestDataPair + { + double test_longitude; + carma_cooperative_perception::UtmZone expected_zone; + }; + + static constexpr std::size_t zone_width{6}; + + // Test data will be {(-180, 1), (-179, 1), (-173, 1), (-167, 2), ..., (180, 60)}; + std::vector test_data{ + {-180, {1, carma_cooperative_perception::Hemisphere::kNorth}}}; + for (std::size_t i{0U}; i < 60UL; ++i) { + test_data.push_back( + {zone_width * i + 1 - 180.0, {i + 1, carma_cooperative_perception::Hemisphere::kNorth}}); + } + + test_data.push_back({180.0, {60, carma_cooperative_perception::Hemisphere::kNorth}}); + + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + using units::literals::operator""_m; + + for (const auto [test_longitude, expected_zone] : test_data) { + const carma_cooperative_perception::Wgs84Coordinate coord{ + 0.0_deg, units::angle::degree_t{test_longitude}, 0.0_m}; + EXPECT_EQ(carma_cooperative_perception::calculate_utm_zone(coord), expected_zone); + } +} + +TEST(ProjectToUtm, BasicCases) +{ + struct TestDataPair + { + carma_cooperative_perception::Wgs84Coordinate test_wgs84; + carma_cooperative_perception::UtmCoordinate expected_utm; + }; + + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + using units::literals::operator""_m; + + std::vector test_data{ + {carma_cooperative_perception::Wgs84Coordinate{61.15880_deg, 10.36924_deg, 25.6_m}, + carma_cooperative_perception::UtmCoordinate{ + {32, carma_cooperative_perception::Hemisphere::kNorth}, 573682.75_m, 6781246.70_m, 25.6_m}}, + + {carma_cooperative_perception::Wgs84Coordinate{-7.96383_deg, 97.00547_deg, 45.7_m}, + carma_cooperative_perception::UtmCoordinate{ + {47, carma_cooperative_perception::Hemisphere::kSouth}, 280142.09_m, 9119170.45_m, 45.7_m}}, + + {carma_cooperative_perception::Wgs84Coordinate{19.93875_deg, -151.15646_deg, -12.1_m}, + carma_cooperative_perception::UtmCoordinate{ + {5, carma_cooperative_perception::Hemisphere::kNorth}, 692944.13_m, 2205762.20_m, -12.1_m}}}; + + auto test_index{0U}; + for (const auto [test_wgs84, expected_utm] : test_data) { + const auto utm{carma_cooperative_perception::project_to_utm(test_wgs84)}; + + EXPECT_EQ(expected_utm.utm_zone, utm.utm_zone) << "Test index: " << test_index; + EXPECT_NEAR( + carma_cooperative_perception::remove_units(expected_utm.easting), + carma_cooperative_perception::remove_units(utm.easting), 1e-2) + << "Test index: " << test_index; + EXPECT_NEAR( + carma_cooperative_perception::remove_units(expected_utm.northing), + carma_cooperative_perception::remove_units(utm.northing), 1e-2) + << "Test index: " << test_index; + EXPECT_NEAR( + carma_cooperative_perception::remove_units(expected_utm.elevation), + carma_cooperative_perception::remove_units(utm.elevation), 1e-2) + << "Test index: " << test_index; + + ++test_index; + } +} + +TEST(CalculateGridConvergence, RightHalf) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + using units::literals::operator""_m; + + const carma_cooperative_perception::UtmZone utm_zone{ + 32, carma_cooperative_perception::Hemisphere::kNorth}; + + // UTM Zone 32: 510500 easting, 7043500 northing + const carma_cooperative_perception::Wgs84Coordinate position_wgs84{ + 63.510617_deg, 9.210989_deg, 25.3_m}; + + const auto result = + carma_cooperative_perception::calculate_grid_convergence(position_wgs84, utm_zone); + + EXPECT_NEAR(carma_cooperative_perception::remove_units(result), 0.188839, 1e-6); +} + +TEST(CalculateGridConvergence, LeftHalf) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + using units::literals::operator""_m; + + const carma_cooperative_perception::UtmZone utm_zone{ + 32, carma_cooperative_perception::Hemisphere::kNorth}; + + // UTM Zone 32: 480500 easting, 7043500 northing + const carma_cooperative_perception::Wgs84Coordinate position_wgs84{ + 63.510617_deg, 8.608168_deg, 25.3_m}; + + const auto result = + carma_cooperative_perception::calculate_grid_convergence(position_wgs84, utm_zone); + + EXPECT_NEAR(carma_cooperative_perception::remove_units(result), -0.350697, 1e-6); +} diff --git a/carma_cooperative_perception/test/test_j2735_types.cpp b/carma_cooperative_perception/test/test_j2735_types.cpp new file mode 100644 index 0000000000..8f140894d7 --- /dev/null +++ b/carma_cooperative_perception/test/test_j2735_types.cpp @@ -0,0 +1,274 @@ +// Copyright 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 + +#include +#include +#include +#include +#include + +TEST(DDateTime, FromJ2735MsgAllAvailable) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_yr; + using units::literals::operator""_d; + using units::literals::operator""_hr; + using units::literals::operator""_min; + using units::literals::operator""_s; + + j2735_v2x_msgs::msg::DDateTime msg; + msg.presence_vector = 0b1111'1111; + msg.year.year = 1; + msg.month.month = 2; + msg.day.day = 3; + msg.hour.hour = 4; + msg.minute.minute = 5; + msg.second.millisecond = 6000; + msg.offset.offset_minute = 7; + + const auto d_date_time{carma_cooperative_perception::DDateTime::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + + ASSERT_TRUE(d_date_time.year.has_value()); + EXPECT_EQ(d_date_time.year.value(), 1_yr); + + ASSERT_TRUE(d_date_time.month.has_value()); + EXPECT_EQ(d_date_time.month.value(), carma_cooperative_perception::February); + + ASSERT_TRUE(d_date_time.day.has_value()); + EXPECT_EQ(d_date_time.day.value(), 3_d); + + ASSERT_TRUE(d_date_time.hour.has_value()); + EXPECT_EQ(d_date_time.hour.value(), 4_hr); + + ASSERT_TRUE(d_date_time.minute.has_value()); + EXPECT_EQ(d_date_time.minute.value(), 5_min); + + ASSERT_TRUE(d_date_time.second.has_value()); + EXPECT_EQ(d_date_time.second.value(), 6_s); + + ASSERT_TRUE(d_date_time.time_zone_offset.has_value()); + EXPECT_EQ(d_date_time.time_zone_offset.value(), 7_min); +} + +TEST(DDateTime, FromJ2735MsgNoneAvailable) +{ + j2735_v2x_msgs::msg::DDateTime msg; + msg.presence_vector = 0b0000'0000; + + const auto d_date_time{carma_cooperative_perception::DDateTime::from_msg(msg)}; + + ASSERT_FALSE(d_date_time.year.has_value()); + ASSERT_FALSE(d_date_time.month.has_value()); + ASSERT_FALSE(d_date_time.day.has_value()); + ASSERT_FALSE(d_date_time.hour.has_value()); + ASSERT_FALSE(d_date_time.minute.has_value()); + ASSERT_FALSE(d_date_time.second.has_value()); + ASSERT_FALSE(d_date_time.time_zone_offset.has_value()); +} + +TEST(AccelerationSet4Way, FromJ2735Msg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_centi_mps_sq; + using units::literals::operator""_two_centi_SG; + using units::literals::operator""_centi_deg_per_s; + + j2735_v2x_msgs::msg::AccelerationSet4Way msg; + msg.longitudinal = 100; // centimeters per second squared + msg.lateral = 200; // centimeters per second squared + msg.vert = 30; // 2 centi G + msg.yaw_rate = 400; // centi degrees per second + + const auto accel_set{carma_cooperative_perception::AccelerationSet4Way::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(accel_set.longitudinal, 100_centi_mps_sq); + EXPECT_EQ(accel_set.lateral, 200_centi_mps_sq); + EXPECT_EQ(accel_set.vert, 30_two_centi_SG); + EXPECT_EQ(accel_set.yaw_rate, 400_centi_deg_per_s); +} + +TEST(AccelerationSet4Way, FromCarmaMsg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_mps_sq; + using units::literals::operator""_deg_per_s; + + carma_v2x_msgs::msg::AccelerationSet4Way msg; + msg.longitudinal = 100; // meters per second squared + msg.lateral = 200; // meters per second squared + msg.vert = 300; // meters per second squared + msg.yaw_rate = 400; // degrees per second + + const auto accel_set{carma_cooperative_perception::AccelerationSet4Way::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(accel_set.longitudinal, 100_mps_sq); + EXPECT_EQ(accel_set.lateral, 200_mps_sq); + EXPECT_EQ(accel_set.vert, 300_mps_sq); + EXPECT_EQ(accel_set.yaw_rate, 400_deg_per_s); +} + +TEST(Position3D, FromJ2735MsgAllAvailable) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deci_udeg; + using units::literals::operator""_deca_cm; + + j2735_v2x_msgs::msg::Position3D msg; + msg.latitude = 100; // deci micro degrees + msg.longitude = 200; // deci micro degrees + msg.elevation_exists = true; + msg.elevation = 300; // deca centimeters + + const auto position{carma_cooperative_perception::Position3D::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.latitude, 100_deci_udeg); + EXPECT_EQ(position.longitude, 200_deci_udeg); + + ASSERT_TRUE(position.elevation.has_value()); + EXPECT_EQ(position.elevation.value(), 300_deca_cm); +} + +TEST(Position3D, FromJ2735MsgNoElevation) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deci_udeg; + + j2735_v2x_msgs::msg::Position3D msg; + msg.latitude = 100; + msg.longitude = 200; + msg.elevation = msg.ELEVATION_UNAVAILABLE; + + const auto position{carma_cooperative_perception::Position3D::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.latitude, 100_deci_udeg); + EXPECT_EQ(position.longitude, 200_deci_udeg); + + EXPECT_FALSE(position.elevation.has_value()); +} + +TEST(Position3D, FromCarmaMsgAllAvailable) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + using units::literals::operator""_m; + + carma_v2x_msgs::msg::Position3D msg; + msg.latitude = 100; // degrees + msg.longitude = 200; // degrees + msg.elevation_exists = true; + msg.elevation = 300; // meters + + const auto position{carma_cooperative_perception::Position3D::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.latitude, 100_deg); + EXPECT_EQ(position.longitude, 200_deg); + + ASSERT_TRUE(position.elevation.has_value()); + EXPECT_EQ(position.elevation.value(), 300_m); +} + +TEST(Position3D, FromCarmaMsgNoElevation) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + + carma_v2x_msgs::msg::Position3D msg; + msg.latitude = 100; + msg.longitude = 200; + msg.elevation = msg.ELEVATION_UNAVAILABLE; + + const auto position{carma_cooperative_perception::Position3D::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.latitude, 100_deg); + EXPECT_EQ(position.longitude, 200_deg); + + EXPECT_FALSE(position.elevation.has_value()); +} + +TEST(Heading, FromJ2735Msg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_eighth_ddeg; + + j2735_v2x_msgs::msg::Heading msg; + msg.heading = 100; // eighth deci degrees + + const auto heading{carma_cooperative_perception::Heading::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(heading.heading, 100_eighth_ddeg); +} + +TEST(Heading, FromCarmaMsg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_deg; + + carma_v2x_msgs::msg::Heading msg; + msg.heading = 100; // degrees + + const auto heading{carma_cooperative_perception::Heading::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(heading.heading, 100_deg); +} + +TEST(Speed, FromJ2735Msg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_two_centi_mps; + + j2735_v2x_msgs::msg::Speed msg; + msg.speed = 100; // 2 cm per second + + const auto speed{carma_cooperative_perception::Speed::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(speed.speed, 100_two_centi_mps); +} + +TEST(Speed, FromCarmaMsg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_mps; + + carma_v2x_msgs::msg::Speed msg; + msg.speed = 100; // meters per second + + const auto speed{carma_cooperative_perception::Speed::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(speed.speed, 100_mps); +} diff --git a/carma_cooperative_perception/test/test_j3224_types.cpp b/carma_cooperative_perception/test/test_j3224_types.cpp new file mode 100644 index 0000000000..0537037908 --- /dev/null +++ b/carma_cooperative_perception/test/test_j3224_types.cpp @@ -0,0 +1,131 @@ +// Copyright 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 + +#include + +TEST(PositionOffsetXYZ, FromJ3224MsgAllAvailable) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_dm; + + j3224_v2x_msgs::msg::PositionOffsetXYZ msg; + msg.offset_x.object_distance = 1; // decimeters + msg.offset_y.object_distance = 2; // decimeters + msg.presence_vector |= msg.HAS_OFFSET_Z; + msg.offset_z.object_distance = 3; // decimeters + + const auto position{carma_cooperative_perception::PositionOffsetXYZ::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.offset_x, 1_dm); + EXPECT_EQ(position.offset_y, 2_dm); + + ASSERT_TRUE(position.offset_z.has_value()); + EXPECT_EQ(position.offset_z, 3_dm); +} + +TEST(PositionOffsetXYZ, FromJ3224MsgNoZOffset) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_dm; + + j3224_v2x_msgs::msg::PositionOffsetXYZ msg; + msg.offset_x.object_distance = 1; // decimeters + msg.offset_y.object_distance = 2; // decimeters + msg.presence_vector &= ~msg.HAS_OFFSET_Z; + + const auto position{carma_cooperative_perception::PositionOffsetXYZ::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.offset_x, 1_dm); + EXPECT_EQ(position.offset_y, 2_dm); + + EXPECT_FALSE(position.offset_z.has_value()); +} + +TEST(PositionOffsetXYZ, FromCarmaMsgAllAvailable) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_m; + + carma_v2x_msgs::msg::PositionOffsetXYZ msg; + msg.offset_x.object_distance = 1; // meters + msg.offset_y.object_distance = 2; // meters + msg.presence_vector |= msg.HAS_OFFSET_Z; + msg.offset_z.object_distance = 3; // meters + + const auto position{carma_cooperative_perception::PositionOffsetXYZ::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.offset_x, 1_m); + EXPECT_EQ(position.offset_y, 2_m); + + ASSERT_TRUE(position.offset_z.has_value()); + EXPECT_EQ(position.offset_z, 3_m); +} + +TEST(PositionOffsetXYZ, FromCarmaMsgNoZOffset) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_m; + + carma_v2x_msgs::msg::PositionOffsetXYZ msg; + msg.offset_x.object_distance = 1; // meters + msg.offset_y.object_distance = 2; // meters + msg.presence_vector &= ~msg.HAS_OFFSET_Z; + + const auto position{carma_cooperative_perception::PositionOffsetXYZ::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(position.offset_x, 1_m); + EXPECT_EQ(position.offset_y, 2_m); + + EXPECT_FALSE(position.offset_z.has_value()); +} + +TEST(MeasurementTimeOffset, FromJ3224Msg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_ms; + + j3224_v2x_msgs::msg::MeasurementTimeOffset msg; + msg.measurement_time_offset = 1; // milliseconds + + const auto offset{carma_cooperative_perception::MeasurementTimeOffset::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(offset.measurement_time_offset, 1_ms); +} + +TEST(MeasurementTimeOffset, FromCarmaMsg) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_s; + + carma_v2x_msgs::msg::MeasurementTimeOffset msg; + msg.measurement_time_offset = 1; // seconds + + const auto offset{carma_cooperative_perception::MeasurementTimeOffset::from_msg(msg)}; + + // Due to nholthaus/units's equality operator implementation, + // this will check almost-equality. + EXPECT_EQ(offset.measurement_time_offset, 1_s); +} diff --git a/carma_cooperative_perception/test/test_month.cpp b/carma_cooperative_perception/test/test_month.cpp new file mode 100644 index 0000000000..85b7c2b9c9 --- /dev/null +++ b/carma_cooperative_perception/test/test_month.cpp @@ -0,0 +1,174 @@ +// Copyright 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 + +#include + +#include +#include + +TEST(Month, UnaryIncrement) +{ + carma_cooperative_perception::Month month{1}; + EXPECT_EQ(month++, carma_cooperative_perception::January); + EXPECT_EQ(month++, carma_cooperative_perception::February); + EXPECT_EQ(month++, carma_cooperative_perception::March); + EXPECT_EQ(month++, carma_cooperative_perception::April); + EXPECT_EQ(month++, carma_cooperative_perception::May); + EXPECT_EQ(month++, carma_cooperative_perception::June); + EXPECT_EQ(month++, carma_cooperative_perception::July); + EXPECT_EQ(month++, carma_cooperative_perception::August); + EXPECT_EQ(month++, carma_cooperative_perception::September); + EXPECT_EQ(month++, carma_cooperative_perception::October); + EXPECT_EQ(month++, carma_cooperative_perception::November); + EXPECT_EQ(month++, carma_cooperative_perception::December); + EXPECT_EQ(month, carma_cooperative_perception::January); + + EXPECT_EQ(++month, carma_cooperative_perception::February); + EXPECT_EQ(++month, carma_cooperative_perception::March); + EXPECT_EQ(++month, carma_cooperative_perception::April); + EXPECT_EQ(++month, carma_cooperative_perception::May); + EXPECT_EQ(++month, carma_cooperative_perception::June); + EXPECT_EQ(++month, carma_cooperative_perception::July); + EXPECT_EQ(++month, carma_cooperative_perception::August); + EXPECT_EQ(++month, carma_cooperative_perception::September); + EXPECT_EQ(++month, carma_cooperative_perception::October); + EXPECT_EQ(++month, carma_cooperative_perception::November); + EXPECT_EQ(++month, carma_cooperative_perception::December); + EXPECT_EQ(++month, carma_cooperative_perception::January); +} + +TEST(Month, UnaryDecrement) +{ + carma_cooperative_perception::Month month{12}; + EXPECT_EQ(month--, carma_cooperative_perception::December); + EXPECT_EQ(month--, carma_cooperative_perception::November); + EXPECT_EQ(month--, carma_cooperative_perception::October); + EXPECT_EQ(month--, carma_cooperative_perception::September); + EXPECT_EQ(month--, carma_cooperative_perception::August); + EXPECT_EQ(month--, carma_cooperative_perception::July); + EXPECT_EQ(month--, carma_cooperative_perception::June); + EXPECT_EQ(month--, carma_cooperative_perception::May); + EXPECT_EQ(month--, carma_cooperative_perception::April); + EXPECT_EQ(month--, carma_cooperative_perception::March); + EXPECT_EQ(month--, carma_cooperative_perception::February); + EXPECT_EQ(month--, carma_cooperative_perception::January); + EXPECT_EQ(month, carma_cooperative_perception::December); + + EXPECT_EQ(--month, carma_cooperative_perception::November); + EXPECT_EQ(--month, carma_cooperative_perception::October); + EXPECT_EQ(--month, carma_cooperative_perception::September); + EXPECT_EQ(--month, carma_cooperative_perception::August); + EXPECT_EQ(--month, carma_cooperative_perception::July); + EXPECT_EQ(--month, carma_cooperative_perception::June); + EXPECT_EQ(--month, carma_cooperative_perception::May); + EXPECT_EQ(--month, carma_cooperative_perception::April); + EXPECT_EQ(--month, carma_cooperative_perception::March); + EXPECT_EQ(--month, carma_cooperative_perception::February); + EXPECT_EQ(--month, carma_cooperative_perception::January); + EXPECT_EQ(--month, carma_cooperative_perception::December); +} + +TEST(Month, UnsignedConversion) +{ + const carma_cooperative_perception::Month month{5}; + EXPECT_EQ(static_cast(month), 5U); +} + +TEST(Month, OkFunction) +{ + { + const carma_cooperative_perception::Month month{0}; + EXPECT_FALSE(month.ok()); + } + { + const carma_cooperative_perception::Month month{1}; + EXPECT_TRUE(month.ok()); + } + { + const carma_cooperative_perception::Month month{12}; + EXPECT_TRUE(month.ok()); + } + { + const carma_cooperative_perception::Month month{13}; + EXPECT_FALSE(month.ok()); + } +} + +TEST(Month, Equality) +{ + const carma_cooperative_perception::Month month{9}; + + EXPECT_EQ(month, carma_cooperative_perception::September); +} + +TEST(Month, NotEquality) +{ + const carma_cooperative_perception::Month month{19}; + + EXPECT_NE(month, carma_cooperative_perception::September); +} + +TEST(Month, LessThan) +{ + const carma_cooperative_perception::Month month{3}; + + EXPECT_LT(month, carma_cooperative_perception::December); +} + +TEST(Month, LessThanEqual) +{ + const carma_cooperative_perception::Month month{3}; + + EXPECT_LE(month, carma_cooperative_perception::March); +} + +TEST(Month, GreaterThan) +{ + const carma_cooperative_perception::Month month{19}; + + EXPECT_GT(month, carma_cooperative_perception::September); +} + +TEST(Month, GreaterThanEqual) +{ + const carma_cooperative_perception::Month month{10}; + + EXPECT_GE(month, carma_cooperative_perception::October); +} + +TEST(MonthFromNumber, Ostream) +{ + static constexpr std::array abbreviations{"Jan.", "Feb.", "Mar.", "Apr.", "May", "Jun.", + "Jul.", "Aug.", "Sep.", "Oct.", "Nov.", "Dec."}; + + { + std::stringstream stream; + stream << carma_cooperative_perception::Month{static_cast(0)}; + EXPECT_EQ(stream.str(), "0 is not a valid month"); + } + + for (auto m{1}; m <= 12; ++m) { + std::stringstream stream; + stream << carma_cooperative_perception::Month{static_cast(m)}; + EXPECT_EQ(stream.str(), abbreviations.at(m - 1)); + } + + { + std::stringstream stream; + stream << carma_cooperative_perception::Month{static_cast(13)}; + EXPECT_EQ(stream.str(), "13 is not a valid month"); + } +} diff --git a/carma_cooperative_perception/test/test_msg_conversion.cpp b/carma_cooperative_perception/test/test_msg_conversion.cpp new file mode 100644 index 0000000000..3ff92c768f --- /dev/null +++ b/carma_cooperative_perception/test/test_msg_conversion.cpp @@ -0,0 +1,523 @@ +// Copyright 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 + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +TEST(ToTimeMsg, HasSeconds) +{ + carma_cooperative_perception::DDateTime d_date_time; + d_date_time.second = units::time::second_t{42.13}; + + builtin_interfaces::msg::Time expected_msg; + expected_msg.sec = 42.0; + expected_msg.nanosec = 130'000'000; + + const auto actual_msg{carma_cooperative_perception::to_time_msg(d_date_time)}; + + EXPECT_DOUBLE_EQ(actual_msg.sec, expected_msg.sec); + EXPECT_DOUBLE_EQ(actual_msg.nanosec, expected_msg.nanosec); +} + +TEST(ToTimeMsg, NulloptSeconds) +{ + const carma_cooperative_perception::DDateTime d_date_time; + + builtin_interfaces::msg::Time expected_msg; + expected_msg.sec = 0.0; + expected_msg.nanosec = 0; + + const auto actual_msg{carma_cooperative_perception::to_time_msg(d_date_time)}; + + EXPECT_DOUBLE_EQ(actual_msg.sec, expected_msg.sec); + EXPECT_DOUBLE_EQ(actual_msg.nanosec, expected_msg.nanosec); +} + +TEST(ToDetectionMsg, Simple) +{ + carma_v2x_msgs::msg::SensorDataSharingMessage sdsm_msg; + sdsm_msg.sdsm_time_stamp.second.millisecond = 1000; + sdsm_msg.sdsm_time_stamp.presence_vector |= sdsm_msg.sdsm_time_stamp.SECOND; + sdsm_msg.ref_pos.longitude = -90.703125; // degrees + sdsm_msg.ref_pos.latitude = 32.801128; // degrees + sdsm_msg.ref_pos.elevation_exists = true; + sdsm_msg.ref_pos.elevation = 300.0; // m + + carma_v2x_msgs::msg::DetectedObjectData object_data; + object_data.detected_object_common_data.detected_id.object_id = 0xBEEF; + object_data.detected_object_common_data.measurement_time.measurement_time_offset = -0.1; // s + + object_data.detected_object_common_data.heading.heading = 34; // true heading; degrees + object_data.detected_object_common_data.obj_type.object_type = + object_data.detected_object_common_data.obj_type.VEHICLE; + + object_data.detected_object_common_data.pos.offset_x.object_distance = 100.0; // m + object_data.detected_object_common_data.pos.offset_y.object_distance = 100.0; // m + + object_data.detected_object_common_data.pos.presence_vector |= + object_data.detected_object_common_data.pos.HAS_OFFSET_Z; + object_data.detected_object_common_data.pos.offset_z.object_distance = 100.0; // m + + object_data.detected_object_common_data.speed.speed = 10; // m/s + object_data.detected_object_common_data.speed_z.speed = 20; // m/s + + object_data.detected_object_common_data.accel_4_way.longitudinal = 0.5; // m/s^2 + object_data.detected_object_common_data.accel_4_way.lateral = 1.00; // m/s^2 + object_data.detected_object_common_data.accel_4_way.vert = 23.536; // m/s^2 + object_data.detected_object_common_data.accel_4_way.yaw_rate = 5.0; // degrees/s + + sdsm_msg.objects.detected_object_data.push_back(object_data); + + const auto detection_list{carma_cooperative_perception::to_detection_list_msg(sdsm_msg)}; + ASSERT_EQ(std::size(detection_list.detections), 1U); + + const auto detection{detection_list.detections.at(0)}; + + EXPECT_EQ(detection.header.stamp.sec, 0); + EXPECT_NEAR(detection.header.stamp.nanosec, 900'000'000U, 2); // +/- 2 ns is probably good enough + EXPECT_EQ(detection.header.frame_id, "15N"); + + EXPECT_NEAR(detection.pose.pose.position.x, 715068.54 + 100.0, 1e-2); // m (ref pos + offset) + EXPECT_NEAR(detection.pose.pose.position.y, 3631576.38 + 100.0, 1e-2); // m (ref pos + offset) + EXPECT_NEAR(detection.pose.pose.position.z, 300.0 + 100.0, 1e-3); // m (ref pos + offset) + EXPECT_DOUBLE_EQ(detection.pose.pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(detection.pose.pose.orientation.y, 0.0); + EXPECT_NEAR(detection.pose.pose.orientation.z, 0.479035, 1e-5); + EXPECT_NEAR(detection.pose.pose.orientation.w, 0.877796, 1e-5); + + EXPECT_DOUBLE_EQ(detection.twist.twist.linear.x, 10.0); + EXPECT_DOUBLE_EQ(detection.twist.twist.linear.y, 0.0); + EXPECT_DOUBLE_EQ(detection.twist.twist.linear.z, 20.0); + EXPECT_DOUBLE_EQ(detection.twist.twist.angular.z, 5.0); + + EXPECT_DOUBLE_EQ(detection.accel.accel.linear.x, 0.5); + EXPECT_DOUBLE_EQ(detection.accel.accel.linear.y, 1.0); + EXPECT_NEAR(detection.accel.accel.linear.z, 2.4 * 9.80665, 1e-4); + + EXPECT_EQ(detection.id, std::to_string(0xBEEF)); + EXPECT_EQ(detection.motion_model, detection.MOTION_MODEL_CTRV); +} + +TEST(CalcDetectionTimeStamp, Simple) +{ + carma_cooperative_perception::DDateTime d_date_time; + d_date_time.second = units::time::second_t{5.0}; + + carma_cooperative_perception::MeasurementTimeOffset offset{units::time::millisecond_t{2}}; + + const auto stamp{carma_cooperative_perception::calc_detection_time_stamp(d_date_time, offset)}; + + ASSERT_TRUE(stamp.second.has_value()); + EXPECT_DOUBLE_EQ(carma_cooperative_perception::remove_units(stamp.second.value()), 5.002); +} + +TEST(ToPositionMsg, Simple) +{ + // Note: Google C++ style guide prohibits namespace using-directives + using units::literals::operator""_m; + + constexpr carma_cooperative_perception::UtmZone zone{ + 32, carma_cooperative_perception::Hemisphere::kNorth}; + constexpr carma_cooperative_perception::UtmCoordinate position_utm{zone, 12.0_m, 13.5_m, -0.5_m}; + const auto position_msg{carma_cooperative_perception::to_position_msg(position_utm)}; + + EXPECT_DOUBLE_EQ( + carma_cooperative_perception::remove_units(position_utm.easting), position_msg.x); + EXPECT_DOUBLE_EQ( + carma_cooperative_perception::remove_units(position_utm.northing), position_msg.y); + EXPECT_DOUBLE_EQ( + carma_cooperative_perception::remove_units(position_utm.elevation), position_msg.z); +} + +// No ToDetectionListMsg test because a DetectionList.msg contains only a list of Detection.msg +// elements, the test for which is covered by ToDetectionMsg. +// TEST(ToDetectionListMsg, Simple) {} + +TEST(ToDetectionMsg, FromExternalObject) +{ + carma_perception_msgs::msg::ExternalObject object; + object.header.stamp.sec = 1; + object.header.stamp.nanosec = 2; + object.header.frame_id = "test_frame"; + object.bsm_id = {3, 4, 5, 6}; + object.id = 7; + object.pose.pose.position.x = 8; + object.pose.pose.position.y = 9; + object.pose.pose.position.z = 10; + object.pose.pose.orientation.x = 11; + object.pose.pose.orientation.y = 12; + object.pose.pose.orientation.z = 13; + object.pose.pose.orientation.w = 14; + object.velocity_inst.twist.linear.x = 15; + object.velocity_inst.twist.linear.y = 16; + object.velocity_inst.twist.linear.z = 17; + object.velocity_inst.twist.angular.x = 18; + object.velocity_inst.twist.angular.y = 19; + object.velocity_inst.twist.angular.z = 20; + object.object_type = object.SMALL_VEHICLE; + + object.presence_vector |= object.BSM_ID_PRESENCE_VECTOR | object.ID_PRESENCE_VECTOR | + object.POSE_PRESENCE_VECTOR | object.VELOCITY_INST_PRESENCE_VECTOR | + object.OBJECT_TYPE_PRESENCE_VECTOR; + + constexpr carma_cooperative_perception::MotionModelMapping motion_model_mapping{ + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV}; + + const auto detection{ + carma_cooperative_perception::to_detection_msg(object, motion_model_mapping)}; + + EXPECT_EQ(detection.header, object.header); + EXPECT_EQ(detection.id, "3456-7"); + EXPECT_EQ(detection.pose, object.pose); + EXPECT_EQ(detection.twist, object.velocity_inst); + EXPECT_EQ(detection.motion_model, detection.MOTION_MODEL_CTRV); +} + +TEST(ToDetectionListMsg, FromExternalObjectList) +{ + carma_perception_msgs::msg::ExternalObjectList object_list; + object_list.objects.emplace_back(); + object_list.objects.emplace_back(); + + constexpr carma_cooperative_perception::MotionModelMapping motion_model_mapping{ + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV, + carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV}; + + const auto detection_list{ + carma_cooperative_perception::to_detection_list_msg(object_list, motion_model_mapping)}; + + EXPECT_EQ(std::size(detection_list.detections), 2U); +} + +TEST(ToExternalObject, FromTrack) +{ + carma_cooperative_perception_interfaces::msg::Track track; + track.header.stamp.sec = 1; + track.header.stamp.nanosec = 2; + track.header.frame_id = "test_frame"; + + track.id = "1234"; + + track.pose.pose.position.x = 1; + track.pose.pose.position.y = 2; + track.pose.pose.position.z = 3; + + track.pose.pose.orientation.x = 4; + track.pose.pose.orientation.y = 5; + track.pose.pose.orientation.z = 6; + track.pose.pose.orientation.w = 7; + + std::iota(std::begin(track.pose.covariance), std::end(track.pose.covariance), 1U); + + track.twist.twist.linear.x = 1; + track.twist.twist.linear.y = 2; + track.twist.twist.linear.z = 3; + + track.twist.twist.angular.x = 4; + track.twist.twist.angular.y = 5; + track.twist.twist.angular.z = 6; + + std::iota(std::begin(track.twist.covariance), std::end(track.twist.covariance), 1U); + + const auto external_object{carma_cooperative_perception::to_external_object_msg(track)}; + + EXPECT_TRUE(external_object.presence_vector & external_object.ID_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.POSE_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.VELOCITY_PRESENCE_VECTOR); + + EXPECT_EQ(external_object.id, 1234U); + + EXPECT_EQ(external_object.header, track.header); + EXPECT_EQ(external_object.pose, track.pose); + EXPECT_EQ(external_object.velocity, track.twist); +} + +TEST(ToExternalObject, FromTrackNonNumericId) +{ + carma_cooperative_perception_interfaces::msg::Track track; + track.header.stamp.sec = 1; + track.header.stamp.nanosec = 2; + track.header.frame_id = "test_frame"; + + track.id = "abcd"; + + track.pose.pose.position.x = 1; + track.pose.pose.position.y = 2; + track.pose.pose.position.z = 3; + + track.pose.pose.orientation.x = 4; + track.pose.pose.orientation.y = 5; + track.pose.pose.orientation.z = 6; + track.pose.pose.orientation.w = 7; + + std::iota(std::begin(track.pose.covariance), std::end(track.pose.covariance), 1U); + + track.twist.twist.linear.x = 1; + track.twist.twist.linear.y = 2; + track.twist.twist.linear.z = 3; + + track.twist.twist.angular.x = 4; + track.twist.twist.angular.y = 5; + track.twist.twist.angular.z = 6; + + std::iota(std::begin(track.twist.covariance), std::end(track.twist.covariance), 1U); + + const auto external_object{carma_cooperative_perception::to_external_object_msg(track)}; + + EXPECT_FALSE(external_object.presence_vector & external_object.ID_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.POSE_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.VELOCITY_PRESENCE_VECTOR); + + EXPECT_EQ(external_object.header, track.header); + EXPECT_EQ(external_object.pose, track.pose); + EXPECT_EQ(external_object.velocity, track.twist); +} + +TEST(ToExternalObject, FromTrackNegativeId) +{ + carma_cooperative_perception_interfaces::msg::Track track; + track.header.stamp.sec = 1; + track.header.stamp.nanosec = 2; + track.header.frame_id = "test_frame"; + + track.id = "-1234"; + + track.pose.pose.position.x = 1; + track.pose.pose.position.y = 2; + track.pose.pose.position.z = 3; + + track.pose.pose.orientation.x = 4; + track.pose.pose.orientation.y = 5; + track.pose.pose.orientation.z = 6; + track.pose.pose.orientation.w = 7; + + std::iota(std::begin(track.pose.covariance), std::end(track.pose.covariance), 1U); + + track.twist.twist.linear.x = 1; + track.twist.twist.linear.y = 2; + track.twist.twist.linear.z = 3; + + track.twist.twist.angular.x = 4; + track.twist.twist.angular.y = 5; + track.twist.twist.angular.z = 6; + + std::iota(std::begin(track.twist.covariance), std::end(track.twist.covariance), 1U); + + const auto external_object{carma_cooperative_perception::to_external_object_msg(track)}; + + EXPECT_FALSE(external_object.presence_vector & external_object.ID_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.POSE_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.VELOCITY_PRESENCE_VECTOR); + + EXPECT_EQ(external_object.header, track.header); + EXPECT_EQ(external_object.pose, track.pose); + EXPECT_EQ(external_object.velocity, track.twist); +} + +TEST(ToExternalObject, FromTrackIdTooLarge) +{ + carma_cooperative_perception_interfaces::msg::Track track; + track.header.stamp.sec = 1; + track.header.stamp.nanosec = 2; + track.header.frame_id = "test_frame"; + + track.id = "5294967295"; + + track.pose.pose.position.x = 1; + track.pose.pose.position.y = 2; + track.pose.pose.position.z = 3; + + track.pose.pose.orientation.x = 4; + track.pose.pose.orientation.y = 5; + track.pose.pose.orientation.z = 6; + track.pose.pose.orientation.w = 7; + + std::iota(std::begin(track.pose.covariance), std::end(track.pose.covariance), 1U); + + track.twist.twist.linear.x = 1; + track.twist.twist.linear.y = 2; + track.twist.twist.linear.z = 3; + + track.twist.twist.angular.x = 4; + track.twist.twist.angular.y = 5; + track.twist.twist.angular.z = 6; + + std::iota(std::begin(track.twist.covariance), std::end(track.twist.covariance), 1U); + + const auto external_object{carma_cooperative_perception::to_external_object_msg(track)}; + + EXPECT_FALSE(external_object.presence_vector & external_object.ID_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.POSE_PRESENCE_VECTOR); + EXPECT_TRUE(external_object.presence_vector & external_object.VELOCITY_PRESENCE_VECTOR); + + EXPECT_EQ(external_object.header, track.header); + EXPECT_EQ(external_object.pose, track.pose); + EXPECT_EQ(external_object.velocity, track.twist); +} + +TEST(ToExternalObjectList, FromTrackList) +{ + carma_cooperative_perception_interfaces::msg::TrackList track_list; + track_list.tracks.push_back(carma_cooperative_perception_interfaces::msg::Track{}); + track_list.tracks.push_back(carma_cooperative_perception_interfaces::msg::Track{}); + track_list.tracks.push_back(carma_cooperative_perception_interfaces::msg::Track{}); + + const auto external_object_list{ + carma_cooperative_perception::to_external_object_list_msg(track_list)}; + + ASSERT_EQ(std::size(external_object_list.objects), 3U); +} + +TEST(ToDetectedObjectDataMsg, FromExternalObject) +{ + carma_perception_msgs::msg::ExternalObject object; + object.header.stamp.sec = 1; + object.header.stamp.nanosec = 2; + object.header.frame_id = "test_frame"; + object.bsm_id = {3, 4, 5, 6}; + object.id = 7; + object.pose.pose.position.x = 8; + object.pose.pose.position.y = 9; + object.pose.pose.position.z = 10; + object.pose.pose.orientation.x = 11; + object.pose.pose.orientation.y = 12; + object.pose.pose.orientation.z = 13; + object.pose.pose.orientation.w = 14; + object.velocity_inst.twist.linear.x = 15; + object.velocity_inst.twist.linear.y = 16; + object.velocity_inst.twist.linear.z = 17; + object.velocity_inst.twist.angular.x = 18; + object.velocity_inst.twist.angular.y = 19; + object.velocity_inst.twist.angular.z = 20; + object.size.x = 21; + object.size.y = 22; + object.size.z = 23; + object.confidence = 0.9; + object.object_type = object.SMALL_VEHICLE; + + object.presence_vector |= object.BSM_ID_PRESENCE_VECTOR | object.ID_PRESENCE_VECTOR | + object.POSE_PRESENCE_VECTOR | object.VELOCITY_INST_PRESENCE_VECTOR | + object.CONFIDENCE_PRESENCE_VECTOR | object.OBJECT_TYPE_PRESENCE_VECTOR | + object.SIZE_PRESENCE_VECTOR; + + std::string proj_string{ + "+proj=tmerc +lat_0=42.24375605014171 +lon_0=-83.55739733422793 +k=1 +x_0=0 +y_0=0 " + "+datum=WGS84 +units=m +vunits=m +no_defs"}; + auto shared_transform = + std::make_shared(proj_string.c_str()); + const auto detected_object{ + carma_cooperative_perception::to_detected_object_data_msg(object, shared_transform)}; + + EXPECT_EQ(detected_object.detected_object_common_data.obj_type.object_type, 1); + EXPECT_EQ(detected_object.detected_object_common_data.obj_type_cfd.classification_confidence, 90); + EXPECT_EQ(detected_object.detected_object_common_data.detected_id.object_id, 7); + EXPECT_NEAR(detected_object.detected_object_common_data.speed.speed, std::sqrt(481), 1e-2); + EXPECT_EQ( + detected_object.detected_object_common_data.speed_z.speed, object.velocity_inst.twist.linear.z); + + EXPECT_EQ(detected_object.detected_object_optional_data.det_veh.size.vehicle_width, 22); + EXPECT_EQ(detected_object.detected_object_optional_data.det_veh.size.vehicle_length, 21); + EXPECT_EQ(detected_object.detected_object_optional_data.det_veh.height.vehicle_height, 23); +} + +TEST(ToSdsmMsg, FromExternalObjectList) +{ + carma_perception_msgs::msg::ExternalObjectList object_list; + object_list.objects.emplace_back(); + object_list.objects.emplace_back(); + + geometry_msgs::msg::PoseStamped current_pose; + current_pose.pose.position.x = 1; + current_pose.pose.position.y = 2; + current_pose.pose.position.z = 3; + + lanelet::projection::LocalFrameProjector local_projector( + "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 " + "+datum=WGS84 +units=m +vunits=m +no_defs"); + std::shared_ptr map_projection; + map_projection = std::make_shared(local_projector); + + const auto sdsm{ + carma_cooperative_perception::to_sdsm_msg(object_list, current_pose, map_projection)}; + + EXPECT_EQ(std::size(sdsm.objects.detected_object_data), 2U); +} + +TEST(ToWgsHeading, FromMapYaw) +{ + std::string proj_string{ + "+proj=tmerc +lat_0=42.24375605014171 +lon_0=-83.55739733422793 +k=1 +x_0=0 +y_0=0 " + "+datum=WGS84 +units=m +vunits=m +no_defs"}; + + // Test conversion from map to lat/lon + // Define map coordinates + lanelet::BasicPoint3d obj_map_coordinates; + obj_map_coordinates.x() = -69.7311856222; + obj_map_coordinates.y() = 331.419278969; + obj_map_coordinates.z() = 37.8485517148; + + double yaw = 10.0; + auto shared_transform = + std::make_shared(proj_string.c_str()); + double heading = carma_cooperative_perception::remove_units( + carma_cooperative_perception::enu_orientation_to_true_heading( + yaw, obj_map_coordinates, shared_transform)); + + EXPECT_EQ(heading, 80); +} + +TEST(toSdsmMsg, getSDSMOffset) +{ + builtin_interfaces::msg::Time external_object_list_stamp; + external_object_list_stamp.sec = 100; + builtin_interfaces::msg::Time external_object_stamp; + external_object_stamp.sec = 50; + auto time_offset = (carma_cooperative_perception::calc_sdsm_time_offset( + external_object_list_stamp, external_object_stamp)) + .measurement_time_offset; + + EXPECT_EQ(time_offset, 50); +} + +TEST(ToSdsmMsg, getRelativePosition) +{ + geometry_msgs::msg::PoseStamped source_pose; + source_pose.pose.position.x = 100; + source_pose.pose.position.y = 100; + carma_v2x_msgs::msg::PositionOffsetXYZ position_offset; + position_offset.offset_x.object_distance = 110; + position_offset.offset_y.object_distance = 110; + + carma_v2x_msgs::msg::PositionOffsetXYZ adjusted_pose = + carma_cooperative_perception::calc_relative_position(source_pose, position_offset); + EXPECT_EQ(adjusted_pose.offset_x.object_distance, 10); +} diff --git a/carma_cooperative_perception/test/track_list_to_external_object_list_launch_test.py b/carma_cooperative_perception/test/track_list_to_external_object_list_launch_test.py new file mode 100644 index 0000000000..585bdd8a1b --- /dev/null +++ b/carma_cooperative_perception/test/track_list_to_external_object_list_launch_test.py @@ -0,0 +1,129 @@ +# Copyright 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. + +import unittest + +from ament_index_python import get_package_share_path + +from carma_launch_testing.transitions import transition_node +from carma_launch_testing.predicates import LenIncreases +from carma_launch_testing.spinning import spin_node_until + +import carma_message_utilities + +from carma_cooperative_perception_interfaces.msg import TrackList +from carma_perception_msgs.msg import ExternalObjectList + +import launch_ros.actions + +from launch import LaunchDescription +from launch.actions import TimerAction + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +from rclpy.context import Context +import rclpy.node + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.track_list_list_pub = self.create_publisher( + TrackList, "input/track_list", 1 + ) + + self.external_object_list_sub = self.create_subscription( + ExternalObjectList, + "output/external_object_list", + lambda msg: self.external_object_list_msgs.append(msg), + 1, + ) + + self.external_object_list_msgs = [] + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = launch_ros.actions.Node( + package="carma_cooperative_perception", + executable="track_list_to_external_object_list_node", + name="node_under_test", + ) + + launch_description = LaunchDescription( + [node_under_test, TimerAction(period=1.0, actions=[ReadyToTest()])] + ) + + return launch_description + + +class TestMotionComputation(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_track_list_conversion(self): + transition_id = Transition.TRANSITION_CONFIGURE + transition_node("node_under_test", transition_id, self.context) + + transition_id = Transition.TRANSITION_ACTIVATE + transition_node("node_under_test", transition_id, self.context) + + package_share_path = get_package_share_path("carma_cooperative_perception") + + msg_file = package_share_path / "test/data/track_list.yaml" + msg = carma_message_utilities.msg_from_yaml_file(msg_file) + self.test_harness_node.track_list_list_pub.publish(msg) + + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.external_object_list_msgs), + self.context, + ) + + self.assertGreaterEqual( + len(self.test_harness_node.external_object_list_msgs), 0 + ) + external_object_list = self.test_harness_node.external_object_list_msgs[-1] + + self.assertEqual(len(external_object_list.objects), 3) + + self.assertEqual(external_object_list.objects[0].id, 1) + self.assertEqual(external_object_list.objects[1].id, 2) + self.assertEqual(external_object_list.objects[2].id, 3) + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/carma_record/scripts/carma_record.sh b/carma_record/scripts/carma_record.sh index 41747f50d0..34046d5a31 100755 --- a/carma_record/scripts/carma_record.sh +++ b/carma_record/scripts/carma_record.sh @@ -2,6 +2,14 @@ while true do + # Get the flag that indiciates whether a ROS 1 rosbag should be recorded + use_ros1_rosbag="$(rosparam get /use_ros1_rosbag)" + if [ "$use_ros1_rosbag" == "false" ] + then + echo "A ROS 1 rosbag will not be recorded" + exit 0 + fi + # Get the regex of the excluded topics excluded_topics="$(rosparam get exclude_regex)" no_exclusions="$(rosparam get no_exclusions)" diff --git a/carma_wm/include/carma_wm/CARMAWorldModel.hpp b/carma_wm/include/carma_wm/CARMAWorldModel.hpp index 47b0ed94db..c84cbae6f5 100644 --- a/carma_wm/include/carma_wm/CARMAWorldModel.hpp +++ b/carma_wm/include/carma_wm/CARMAWorldModel.hpp @@ -15,7 +15,7 @@ * License for the specific language governing permissions and limitations under * the License. */ - +#include #include "carma_wm/WorldModel.hpp" #include #include @@ -110,8 +110,9 @@ class CARMAWorldModel : public WorldModel * @brief processSpatFromMsg update map's traffic light states with SPAT msg * * @param spat_msg Msg to update with + * @param use_sim_time Boolean to indicate if it is currently simulation or not */ - void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg); + void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false); /** * \brief This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane. @@ -135,16 +136,8 @@ class CARMAWorldModel : public WorldModel * \param moy_exists tells weather minute of the year exist or not * \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); - - /*! \brief for cheking previous rate to avoid repetation. - * \param min_end_time_dynamic dynamic spat processing minimum end time - * \param received_state_dynamic phase rate of the movement event list event state - * \param mov_id id of the traffic signal states - * \param mov_signal_group signal group of the traffic signal states - */ - bool check_if_seen_before_movement_state(boost::posix_time::ptime min_end_time_dynamic,lanelet::CarmaTrafficSignalState received_state_dynamic,uint16_t mov_id, uint8_t mov_signal_group); - + 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 * diff --git a/carma_wm/include/carma_wm/SignalizedIntersectionManager.hpp b/carma_wm/include/carma_wm/SignalizedIntersectionManager.hpp index d30859e84d..ff5efc5229 100644 --- a/carma_wm/include/carma_wm/SignalizedIntersectionManager.hpp +++ b/carma_wm/include/carma_wm/SignalizedIntersectionManager.hpp @@ -177,6 +177,7 @@ class SignalizedIntersectionManager // Max width of lane in meters double max_lane_width_ = 4; + }; } // namespace carma_wm \ No newline at end of file diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 6be66ce3e1..36d3fa3194 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -13,7 +13,7 @@ * License for the specific language governing permissions and limitations under * the License. */ -#include + #include #include #include @@ -1417,48 +1417,7 @@ namespace carma_wm return curr_light; } - bool CARMAWorldModel::check_if_seen_before_movement_state(boost::posix_time::ptime min_end_time_dynamic,lanelet::CarmaTrafficSignalState received_state_dynamic,uint16_t mov_id, uint8_t mov_signal_group) - { - - if(sim_.traffic_signal_states_[mov_id][mov_signal_group].empty()) - { - return false; - } - - // temp states that does not include outdated states - std::vector> temp_signal_states; - std::vector temp_start_times; - - int i = 0; - for(auto mov_check:sim_.traffic_signal_states_[mov_id][mov_signal_group]) - { - if (lanelet::time::timeFromSec(std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count()) < mov_check.first) - { - temp_signal_states.push_back(std::make_pair(mov_check.first, mov_check.second )); - temp_start_times.push_back(sim_.traffic_signal_start_times_[mov_id][mov_signal_group][i]); - } - else - { - i++; - continue; - } - - auto last_time_difference = mov_check.first - min_end_time_dynamic; - bool is_duplicate = last_time_difference.total_milliseconds() >= -500 && last_time_difference.total_milliseconds() <= 500; - - if(received_state_dynamic == mov_check.second && is_duplicate) - { - return true; - } - i++; - } - sim_.traffic_signal_states_[mov_id][mov_signal_group]=temp_signal_states; - sim_.traffic_signal_start_times_[mov_id][mov_signal_group] = temp_start_times; - return false; - - } - - 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) + 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) { if (moy_exists) //account for minute of the year { @@ -1467,6 +1426,11 @@ namespace carma_wm auto curr_time_boost = inception_boost + duration_since_inception; int curr_year = curr_time_boost.date().year(); + + // Force the current year to start of epoch if it is simulation + if (is_simulation) + 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")); auto curr_minute_stamp_boost = curr_year_start_boost + boost::posix_time::minutes((int)moy); @@ -1487,7 +1451,7 @@ namespace carma_wm } } - void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg) + void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg, bool use_sim_time) { if (!semantic_map_) { @@ -1503,16 +1467,6 @@ namespace carma_wm for (const auto& curr_intersection : spat_msg.intersection_state_list) { - bool is_dynamic_spat = false; - - for (const auto& current_movement_state : curr_intersection.movement_list) - { - if (current_movement_state.movement_event_list.size() > 1) - { - is_dynamic_spat = true; // if only one of the signal_group is dynamic, then rest is as well - break; - } - } for (const auto& current_movement_state : curr_intersection.movement_list) { @@ -1534,6 +1488,7 @@ namespace carma_wm if (curr_light->revision_ != curr_intersection.revision) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Received a new intersection geometry. intersection_id: " << (int)curr_intersection.id.id << ", and signal_group_id: " << (int)current_movement_state.signal_group); + sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group].clear(); sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].clear(); } @@ -1550,173 +1505,31 @@ namespace carma_wm curr_light->revision_ = curr_intersection.revision; // valid SPAT msg - if(is_dynamic_spat) // Dynamic Spat Processing with future phases - { - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group]={}; - sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group]={}; - - for(auto current_movement_event:current_movement_state.movement_event_list) - { - // raw min_end_time in seconds measured from the most recent full hour - 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); // 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); // Accounting minute of the year + sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group]={}; + sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group]={}; - auto received_state_dynamic = static_cast(current_movement_event.event_state.movement_phase_state); - - //bool recorded = check_if_seen_before_movement_state(min_end_time_dynamic,received_state_dynamic,curr_intersection.id.id,current_movement_state.signal_group); - - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time_dynamic, received_state_dynamic)); - sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group].push_back( - start_time_dynamic); - - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "intersection id: " << (int)curr_intersection.id.id << ", signal: " << (int)current_movement_state.signal_group - << ", start_time: " << std::to_string(lanelet::time::toSec(start_time_dynamic)) - << ", end_time: " << std::to_string(lanelet::time::toSec(min_end_time_dynamic)) - << ", state: " << received_state_dynamic); - } - curr_light->recorded_time_stamps = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group]; - curr_light->recorded_start_time_stamps = sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group]; - } - else // Fixed Spat Processing without future phases + for(auto current_movement_event:current_movement_state.movement_event_list) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Detected fixed cycle as there was no more than 1 future phases! for inter id: " << (int)curr_intersection.id.id << ", signal: " << (int)current_movement_state.signal_group); - // raw min_end_time in seconds measured from the most recent full hour - boost::posix_time::ptime min_end_time = lanelet::time::timeFromSec(current_movement_state.movement_event_list[0].timing.min_end_time); - auto received_state = static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state); - - min_end_time=min_end_time_converter_minute_of_year(min_end_time,curr_intersection.moy_exists,curr_intersection.moy); - - auto last_time_difference = sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group].first - min_end_time; - bool is_duplicate = last_time_difference.total_milliseconds() >= -500 && last_time_difference.total_milliseconds() <= 500; - - //if same data as last time (duplicate or outdated message): - //where state is same and timestamp is equal or less, skip - if (sim_.last_seen_state_.find(curr_intersection.id.id) != sim_.last_seen_state_.end() && - sim_.last_seen_state_[curr_intersection.id.id].find(current_movement_state.signal_group) != sim_.last_seen_state_[curr_intersection.id.id].end() && - is_duplicate) - { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Duplicate as last time! : id: " << curr_light->id() << ", time: " << std::to_string(lanelet::time::toSec(min_end_time))); - continue; - } - - // if received same state as last time, but with new time_stamp in the future, combine the info with last state - // also skip setting state until received a new state that is different from last recorded one - if ( sim_.last_seen_state_.find(curr_intersection.id.id) != sim_.last_seen_state_.end() && - sim_.last_seen_state_[curr_intersection.id.id].find(current_movement_state.signal_group) != sim_.last_seen_state_[curr_intersection.id.id].end() && - sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group].second == received_state && - sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group].first < min_end_time) - { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Updated time for id: " << curr_light->id() << " with state: " << received_state << ", with time: " - << std::to_string(lanelet::time::toSec(min_end_time))); - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first = min_end_time; - continue; - } + 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); - // detected that new state received; therefore, set the last recorded state (not new one received) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Received new state for light: " << curr_light_id << ", with state: " << received_state << ", time: " << rclcpp::Time(boost::posix_time::to_time_t(min_end_time), 0.0).seconds()); + 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 - // update last seen signal state - sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group] = {min_end_time, received_state}; + auto received_state_dynamic = static_cast(current_movement_event.event_state.movement_phase_state); - if (!curr_light->recorded_time_stamps.empty()) - { - boost::posix_time::time_duration time_difference = curr_light->predictState(min_end_time - lanelet::time::durationFromSec(0.5)).get().first - min_end_time; //0.5s to account for error - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Initial time_difference: " << (double)(time_difference.total_milliseconds() / 1000.0)); - - if (curr_light->predictState(min_end_time - lanelet::time::durationFromSec(0.5)).get().second != received_state) - { - // shift to same state's end - boost::posix_time::time_duration shift_to_match_state = curr_light->fixed_cycle_duration - curr_light->signal_durations[received_state]; - time_difference += shift_to_match_state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Time_difference new: " << (double)(time_difference.total_milliseconds() / 1000.0)); - } + sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time_dynamic, received_state_dynamic)); + sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group].push_back( + start_time_dynamic); - // if |time difference| is less than 0.5 sec - bool same_time_stamp_as_last = time_difference.total_milliseconds() >= -500 && time_difference.total_milliseconds() <= 500; - - // Received same cycle info while signal already has full cycle, then skip - if (curr_light->predictState(min_end_time - lanelet::time::durationFromSec(0.5)).get().second == received_state && - same_time_stamp_as_last && - sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] > 4 ) // checking >4 because: 3 unique + 1 more state to - // complete cycle. And last state (e.g. 4th) is updated on next (e.g 5th) - { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Received same cycle info, ignoring : " << std::to_string(lanelet::time::toSec(min_end_time))); - continue; - } - // Received new cycle info after full cycle was set - else if(sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] > 4) - { - for ( auto pair : sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group]) - { - pair.first = pair.first - time_difference; - } - - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group] = {}; - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time, received_state)); - sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] = 1; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Detected new cycle info! Shifted everything! : " << std::to_string(lanelet::time::toSec(min_end_time)) << ", time_difference sec:" << time_difference.total_seconds()); - continue; - } - } - if (sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].size() >= 2 && sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].front().second == - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second) - { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Setting last recorded state for light: " << curr_light_id << ", with state: " << sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second << ", time: " << sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first); - curr_light->setStates(sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group], curr_intersection.revision); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "SUCCESS!: Set new cycle of total seconds: " << lanelet::time::toSec(curr_light->fixed_cycle_duration)); - } - else if (curr_light->recorded_time_stamps.empty()) // if it was never initialized, do its best to plan with the current state until the future state is also received. - { - std::vector> default_state; - // green 20sec, yellow 3sec, red 20sec, back to green 20sec etc... - default_state.push_back(std::make_pair(boost::posix_time::from_time_t(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); - default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE)); - default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(RED_LIGHT_DURATION), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); - default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(GREEN_LIGHT_DURATION), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); - - curr_light->setStates(default_state, curr_intersection.revision); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Set default cycle of total seconds: " << lanelet::time::toSec(curr_light->fixed_cycle_duration)); - } - else if (sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].size() >= 1) - { - auto green_light_duration = lanelet::time::durationFromSec(GREEN_LIGHT_DURATION); - auto yellow_light_duration = lanelet::time::durationFromSec(YELLOW_LIGHT_DURATION); - auto red_light_duration = lanelet::time::durationFromSec(RED_LIGHT_DURATION); - - std::vector> partial_states; - // set the partial cycle. - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Setting last recorded state for light: " << curr_light_id << ", with state: " << sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second << ", time: " << sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first); - for (size_t i = 0; i < sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].size() - 1; i++) - { - auto light_state = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].second; - - if (light_state == lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN || light_state == lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED) - red_light_duration = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; - - else if (light_state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED || light_state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) - green_light_duration = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; - - else if (light_state == lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE || light_state == lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE) - yellow_light_duration = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; - } - - partial_states.push_back(std::make_pair(boost::posix_time::from_time_t(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); - partial_states.push_back(std::make_pair(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE)); - partial_states.push_back(std::make_pair(partial_states.back().first + red_light_duration, lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); - partial_states.push_back(std::make_pair(partial_states.back().first + green_light_duration, lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); - curr_light->setStates(partial_states, curr_intersection.revision); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Set new partial cycle of total seconds: " << lanelet::time::toSec(curr_light->fixed_cycle_duration) << ", for id: "<< curr_light_id << ", " << partial_states.front().second << ", " << partial_states.back().second); - } - - // record the new state received - sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time, received_state)); - sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group]++; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Counter now: " << sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] << ", for id: "<< curr_light_id); - } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "intersection id: " << (int)curr_intersection.id.id << ", signal: " << (int)current_movement_state.signal_group + << ", start_time: " << std::to_string(lanelet::time::toSec(start_time_dynamic)) + << ", end_time: " << std::to_string(lanelet::time::toSec(min_end_time_dynamic)) + << ", state: " << received_state_dynamic); + } + curr_light->recorded_time_stamps = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group]; + curr_light->recorded_start_time_stamps = sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group]; } } } diff --git a/carma_wm/src/WMListener.cpp b/carma_wm/src/WMListener.cpp index 47a0dc252c..2618b7830d 100644 --- a/carma_wm/src/WMListener.cpp +++ b/carma_wm/src/WMListener.cpp @@ -45,16 +45,28 @@ WMListener::WMListener( rclcpp::ParameterValue participant_param_value; participant_param_value = node_params_->declare_parameter("vehicle_participant_type", rclcpp::ParameterValue("")); } + + //Declare parameter if it doesn't exist + rclcpp::Parameter use_sim_time_param("use_sim_time"); + if(!node_params_->get_parameter("use_sim_time", use_sim_time_param)){ + 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()); + RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is using simulation time? : " << use_sim_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()); rclcpp::SubscriptionOptions map_update_options; rclcpp::SubscriptionOptions map_options; diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index 546838f03e..8beb6736dd 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -52,6 +52,7 @@ WorldModelConstPtr WMListenerWorker::getWorldModel() const return std::static_pointer_cast(world_model_); // Cast pointer to const variant } + void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg) { current_map_version_ = map_msg->map_version; @@ -102,7 +103,7 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::Un void WMListenerWorker::incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::UniquePtr spat_msg) { - world_model_->processSpatFromMsg(*spat_msg); + world_model_->processSpatFromMsg(*spat_msg, use_sim_time_); } bool WMListenerWorker::checkIfReRoutingNeeded() const @@ -689,6 +690,11 @@ void WMListenerWorker::setConfigSpeedLimit(double config_lim) world_model_->setConfigSpeedLimit(config_speed_limit_); } +void WMListenerWorker::isUsingSimTime(bool use_sim_time) +{ + use_sim_time_ = use_sim_time; +} + double WMListenerWorker::getConfigSpeedLimit() const { return config_speed_limit_; diff --git a/carma_wm/src/WMListenerWorker.hpp b/carma_wm/src/WMListenerWorker.hpp index 59614ade58..0c347b4172 100644 --- a/carma_wm/src/WMListenerWorker.hpp +++ b/carma_wm/src/WMListenerWorker.hpp @@ -133,12 +133,17 @@ class WMListenerWorker /** * \brief incoming spat message - * */ void incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::UniquePtr spat_msg); + /** + * \brief set true if simulation_mode is on + */ + void isUsingSimTime(bool use_sim_time); + private: std::shared_ptr world_model_; + bool use_sim_time_; std::function map_callback_; std::function route_callback_; void newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement* regem) const; diff --git a/carma_wm/test/CARMAWorldModelTest.cpp b/carma_wm/test/CARMAWorldModelTest.cpp index c2a01826d7..294a9e284d 100755 --- a/carma_wm/test/CARMAWorldModelTest.cpp +++ b/carma_wm/test/CARMAWorldModelTest.cpp @@ -1333,143 +1333,62 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) // call the processSpatFromMsg with that msg 1 event.event_state.movement_phase_state = 5; event.timing.min_end_time = 20; + event.timing.start_time = 0; movement.movement_event_list.push_back(event); state.movement_list.push_back(movement); spat.intersection_state_list.push_back(state); cmw.processSpatFromMsg(spat); auto lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // default - EXPECT_EQ(lanelet::time::durationFromSec(43), lights1[0]->fixed_cycle_duration); - - // call the processSpatFromMsg with that msg 2 - event.event_state.movement_phase_state = 7; - event.timing.min_end_time = 24; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // nothing changed - EXPECT_EQ(lanelet::time::durationFromSec(43), lights1[0]->fixed_cycle_duration); - - // call the processSpatFromMsg with that msg 3a - event.event_state.movement_phase_state = 3; - event.timing.min_end_time = 44.5; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // partial state 7 - EXPECT_EQ(lanelet::time::durationFromSec(44.0), lights1[0]->fixed_cycle_duration); - - // call the processSpatFromMsg with that msg 3b - event.event_state.movement_phase_state = 3; - event.timing.min_end_time = 45.0; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // partial state 7 - EXPECT_EQ(lanelet::time::durationFromSec(44.0), lights1[0]->fixed_cycle_duration); + // By default, traffic_signal shouldn't have fixed_cycle_duration + EXPECT_EQ(lanelet::time::durationFromSec(0), lights1[0]->fixed_cycle_duration); - // call the processSpatFromMsg with that msg 4.a - event.event_state.movement_phase_state = 5; - event.timing.min_end_time = 65; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // partial state 3 - EXPECT_EQ(lanelet::time::durationFromSec(44.5), lights1[0]->fixed_cycle_duration); // NOTE: 44.5 here instead of 45 because of the intentional subtraction of 0.5s inside the implementation - - // call the processSpatFromMsg with that msg 4.b - event.event_state.movement_phase_state = 5; - event.timing.min_end_time = 65.001; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Input: Duplicate, so ignore"); - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc - EXPECT_EQ(lanelet::time::durationFromSec(44.5), lights1[0]->fixed_cycle_duration); // NOTE: 44.5 here instead of 45 because of the intentional subtraction of 0.5s inside the implementation - - // call the processSpatFromMsg with that msg 5 - event.event_state.movement_phase_state = 3; - event.timing.min_end_time = 66; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Input: First cycle set. This is technically new cycle, but this info is not counted towards it due to inconvenience"); + // Do nothing if received SPAT of another intersection + state.id.id = 2; + spat.intersection_state_list.push_back(state); cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // same duration, but counter set to 0 - EXPECT_EQ(lanelet::time::durationFromSec(45), lights1[0]->fixed_cycle_duration); + EXPECT_EQ(lanelet::time::durationFromSec(0), lights1[0]->fixed_cycle_duration); + EXPECT_EQ(1, lights1[0]->recorded_time_stamps.size()); + EXPECT_EQ(1, lights1[0]->recorded_start_time_stamps.size()); - // call the processSpatFromMsg with that msg 6 - // new cycle, but will be counted as old cycle due to same light - event.event_state.movement_phase_state = 5; - event.timing.min_end_time = 67; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Input: New cycle, but cycle duration is same due to shifting"); + // Valid dynamic spat + state.id.id = 1; + spat.intersection_state_list = {}; + spat.intersection_state_list.push_back(state); cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - - EXPECT_EQ(lanelet::time::durationFromSec(45), lights1[0]->fixed_cycle_duration); - - // call the processSpatFromMsg with that msg 7 - event.event_state.movement_phase_state = 7; - event.timing.min_end_time = 68; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"),"Input: New partial cycle, yellow reduced"); + EXPECT_EQ(lanelet::time::durationFromSec(0), lights1[0]->fixed_cycle_duration); + EXPECT_EQ(1, lights1[0]->recorded_time_stamps.size()); + EXPECT_EQ(1, lights1[0]->recorded_start_time_stamps.size()); + EXPECT_EQ(20, lanelet::time::toSec(lights1[0]->recorded_time_stamps.front().first)); + EXPECT_EQ(0, lanelet::time::toSec(lights1[0]->recorded_start_time_stamps.front())); + EXPECT_EQ(lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED, lights1[0]->recorded_time_stamps.front().second); + + // Empty movement, stay the same + state.id.id = 1; + carma_v2x_msgs::msg::MovementState empty_movement; + spat.intersection_state_list[0].movement_list[0] = empty_movement; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc - EXPECT_EQ(lanelet::time::durationFromSec(43), lights1[0]->fixed_cycle_duration); + EXPECT_EQ(1, lights1[0]->recorded_time_stamps.size()); + EXPECT_EQ(1, lights1[0]->recorded_start_time_stamps.size()); - // call the processSpatFromMsg with that msg 8 + // Multiple states + // first state + spat.intersection_state_list[0] = state; + // second state event.event_state.movement_phase_state = 3; - event.timing.min_end_time = 69.000; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Input: New partial cycle, green reduced"); - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc - EXPECT_EQ(lanelet::time::durationFromSec(41), lights1[0]->fixed_cycle_duration); - - // call the processSpatFromMsg with that msg 9 - event.event_state.movement_phase_state = 5; - event.timing.min_end_time = 70; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Input: New full cycle, yellow reduced"); - cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc - EXPECT_EQ(lanelet::time::durationFromSec(22), lights1[0]->fixed_cycle_duration); - - // call the processSpatFromMsg with that msg 10 - event.event_state.movement_phase_state = 7; - event.timing.min_end_time = 71; - movement.movement_event_list[0] = event; - state.movement_list[0] = movement; - spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Input: New full cycle, red reduced"); + event.timing.min_end_time = 40; + event.timing.start_time = 20; + movement.movement_event_list.push_back(event); + state.movement_list.push_back(movement); + spat.intersection_state_list.push_back(state); cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc - EXPECT_EQ(lanelet::time::durationFromSec(3), lights1[0]->fixed_cycle_duration); - + EXPECT_EQ(2, lights1[0]->recorded_time_stamps.size()); + EXPECT_EQ(2, lights1[0]->recorded_start_time_stamps.size()); + EXPECT_NEAR(20.0, lanelet::time::toSec(lights1[0]->recorded_time_stamps.front().first), 0.0001); + EXPECT_NEAR(0.0, lanelet::time::toSec(lights1[0]->recorded_start_time_stamps.front()), 0.0001); + EXPECT_EQ(lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED, lights1[0]->recorded_time_stamps.front().second); + EXPECT_NEAR(40.0, lanelet::time::toSec(lights1[0]->recorded_time_stamps.back().first), 0.0001); + EXPECT_NEAR(20.0, lanelet::time::toSec(lights1[0]->recorded_start_time_stamps.back()), 0.0001); + EXPECT_EQ(lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN, lights1[0]->recorded_time_stamps.back().second); } TEST(CARMAWorldModelTest, getSignalsAlongRoute) @@ -1545,26 +1464,4 @@ TEST(CARMAWorldModelTest, getIntersectionAlongRoute) } -TEST(CARMAWorldModelTest, checkIfSeenBeforeMovementState) -{ - carma_wm::CARMAWorldModel cmw; - auto system_now = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count() + 1.0; - - cmw.sim_.traffic_signal_states_[13][15].push_back(std::make_pair(lanelet::time::timeFromSec(system_now + 1.0), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); - cmw.sim_.traffic_signal_start_times_[13][15].push_back(lanelet::time::timeFromSec(system_now)); - - boost::posix_time::ptime min_end_time_dynamic = lanelet::time::timeFromSec(system_now + 1.0); - auto received_state_dynamic=lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN; - int mov_id=13; - int mov_signal_group=15; - - ASSERT_EQ(cmw.check_if_seen_before_movement_state(min_end_time_dynamic,received_state_dynamic,mov_id,mov_signal_group), 1); - - min_end_time_dynamic=lanelet::time::timeFromSec(system_now); - received_state_dynamic= lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE; - mov_id=13; - mov_signal_group=15; - - ASSERT_EQ(cmw.check_if_seen_before_movement_state(min_end_time_dynamic,received_state_dynamic,mov_id,mov_signal_group), 0); -} } // namespace carma_wm diff --git a/docker/checkout.bash b/docker/checkout.bash index 8e3aa1ddc1..85627fdeea 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -1,13 +1,13 @@ #!/bin/bash -# Copyright (C) 2018-2021 LEIDOS. -# +# Copyright (C) 2018-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 @@ -53,10 +53,12 @@ fi # Get humble branch of message filters which supports template Node arguments (foxy version supports rclcpp::Node only) git clone https://github.com/usdot-fhwa-stol/carma-message-filters.git --branch develop +git clone https://github.com/usdot-fhwa-stol/multiple_object_tracking --branch develop + # add astuff messages -# NOTE: The ibeo_msgs package is ignored because on build the cmake files in that package run a sed command +# NOTE: The ibeo_msgs package is ignored because on build the cmake files in that package run a sed command # which can make them incompatible with a new ros version after a source switch -git clone https://github.com/astuff/astuff_sensor_msgs +git clone https://github.com/astuff/astuff_sensor_msgs cd astuff_sensor_msgs git checkout 41d5ef0c33fb27eb3c9ba808b51332bcce186a83 @@ -69,10 +71,32 @@ echo "" > COLCON_IGNORE cd ../ +# Clone the foxy branch of ros2_tracing in order to enable certain analyses of CARMA Platform +# made possible through collected trace data, such as analyzing ROS 2 callback durations. +git clone -b foxy https://github.com/ros2/ros2_tracing + #rosbridge_suite is a ROS meta-package including all the rosbridge packages. # NOTE: clone -b flag is used instead of --branch to avoid hook rewriting it git clone -b ros2 https://github.com/usdot-fhwa-stol/rosbridge_suite +# The feature/integrate-carma branch of rosbag2 includes improvements that were not possible to backport into the foxy branch +# of rosbag2. These rosbag2 packages will replace the originally built foxy rosbag2 packages. +# NOTE: Additional information regarding the rosbag2 improvements on this branch are included in the forked repository's README. +git clone -b carma-develop https://github.com/usdot-fhwa-stol/rosbag2 + +# Novatel OEM7 Driver +# NOTE: This is required since otherwise this image will not contain the novatel_oem7_msgs package, and a missing ROS 2 message package +# can cause ROS 2 rosbag logging to fail in Foxy. +# Related GitHub discussion for fix that was not backported to Foxy: https://github.com/ros2/rosbag2/pull/858 +git clone https://github.com/novatel/novatel_oem7_driver.git ${dir}/src/novatel_oem7_driver -b ros2-dev +# Checkout verified commit +cd ${dir}/src/novatel_oem7_driver +git checkout 3055e220bb9715b59c3ef53ab0aba05a495d9d5 +# Ignore novatel_oem7_driver package; only novatel_oem7_msgs is required +cd ${dir}/src/novatel_oem7_driver/src/novatel_oem7_driver +echo "" > COLCON_IGNORE +cd ${dir}/src + cd ${dir}/src diff --git a/docker/install.sh b/docker/install.sh index eafde7f833..834e14d628 100755 --- a/docker/install.sh +++ b/docker/install.sh @@ -1,6 +1,6 @@ #!/bin/bash -# Copyright (C) 2018-2021 LEIDOS. +# Copyright (C) 2018-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 @@ -40,13 +40,13 @@ sudo chgrp carma /opt/carma # Set group to expose permissions for build if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then if [[ ! -z "$ROS1_PACKAGES" ]]; then echo "Incrementally building ROS1 packages: $ROS1_PACKAGES" - colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS1_PACKAGES + colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS1_PACKAGES else echo "Build type is incremental but no ROS1 packages specified, skipping ROS1 build..." fi else echo "Building all ROS1 CARMA Components" - colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release + colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip novatel_oem7_msgs tracetools tracetools_test fi echo "Build of ROS1 CARMA Components Complete" diff --git a/engineering_tools/carma_script_extensions/cloud-sim b/engineering_tools/carma_script_extensions/cloud-sim index a2515749aa..c146687086 100755 --- a/engineering_tools/carma_script_extensions/cloud-sim +++ b/engineering_tools/carma_script_extensions/cloud-sim @@ -1,6 +1,6 @@ #!/bin/bash -# Copyright (C) 2022 LEIDOS. +# Copyright (C) 2022-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 @@ -158,104 +158,165 @@ cloud-sim__start() { # Call cloud formation local STACK_NAME="CloudSimStack"$(echo ${USER_ARN} | grep -oh "/.*@" | sed "s|@|-|g" | sed "s|\.|-|g" | sed "s|[^A-Za-z0-9]|-|g") - - local RESULT=$(aws --region us-east-1 cloudformation create-stack --template-body "file://${CARMA_CLOUD_SIM_CLOUD_FORMATION_PATH}" \ - --stack-name "${STACK_NAME}" \ - --parameters ParameterKey=Cloud9AccessRoleName,ParameterValue="${USER_ARN}" \ - ParameterKey=Password,ParameterValue="${PASS}" \ - ParameterKey=CarmaConfigImage,ParameterValue="${CONFIG_IMAGE}" \ - --capabilities CAPABILITY_NAMED_IAM) - local VALID_LAUNCH=$(echo "${RESULT}" | grep 'arn:aws:cloudformation:us-east-1') - if [ -z "${VALID_LAUNCH}" ]; then - echo "AWS Launch Failed with error:" - echo ${RESULT} - exit 1 - fi + # Check if required resources are available + if query_resource_status; then - echo "AWS Stack ${STACK_NAME} starting up......." + # Get the VPC ID + local VPC_ID=$(aws ec2 describe-vpcs --filters "Name=tag:Name,Values=CDASim VPC" --query 'Vpcs[].VpcId' --output text) + # Get the Route Table ID + local ROUTETABLE_ID=$(aws ec2 describe-route-tables --filters "Name=tag:Name,Values=CDASim RouteTable" --query 'RouteTables[].RouteTableId' --output text) - local STACK_RESOURCE_STATUS="NOT_EMPTY" - local LAUNCHED_RESOURCE_COUNT=0 - local PREV_LAUNCH_COUNT=0 - local i=0 - local SLEEP_SEC=5 - local LAUNCH_TIMEOUT=595 # 10m timeout to launch the stack - local ELAPSED_TIME=0 - local NON_COMPLETE_RES="ALL" - local FAILED_RES="NONE" - local TOTAL_STACK_RESOURCE=0 + # Get subnet information + local SubnetCIDRBlock="10.0.0.0/24" + + # Query for existing subnetID list + local subnet_ids=$(aws ec2 describe-route-tables --route-table-ids "$ROUTETABLE_ID" \ + --query 'RouteTables[0].Associations[].SubnetId' \ + --output text) + # Use default subnet if none exist, else check existing subnets and increment by 1 + local total_subnets=${#subnet_ids[@]} + - sleep ${SLEEP_SEC} + if [ -n "${subnet_ids[0]}" ]; then + local max=0 + for ((i = 0; i < total_subnets; i++)); do + subnet_id=${subnet_ids[i]} - # As long as there are resources which do not have the status CREATE_COMPLETE - # TODO: Ideally we wouldn't need to hard code 14 resources but a rare race condition can occur where 13 complete before 14 starts which leads to the need to hard code it for now - while { [ -n "${NON_COMPLETE_RES}" ] || [ ${TOTAL_STACK_RESOURCE} -lt 14 ]; } && [ ${ELAPSED_TIME} -lt ${LAUNCH_TIMEOUT} ]; do + # Lookup subnet address + cidr_block=$(aws ec2 describe-subnets --subnet-ids "$subnet_id" --query 'Subnets[0].CidrBlock' --output text) + + # Split the CIDR block into its components + IFS="/" read -r ip_address subnet_mask <<< "$cidr_block" - STACK_RESOURCE_STATUS=$(aws cloudformation describe-stack-resources --stack-name "${STACK_NAME}" | grep -oh '"ResourceStatus":.*",' | sed "s|\s*\"ResourceStatus\": \"||" | sed "s|\",||") + # Split the IP address into its octets + IFS="." read -r -a octets <<< "$ip_address" - NON_COMPLETE_RES=$(echo "${STACK_RESOURCE_STATUS}" | grep -v "CREATE_COMPLETE" | sed "s/\n//" ) - FAILED_RES=$(echo "${STACK_RESOURCE_STATUS}" | grep "\(ROLLBACK\|FAILED\|DELETE\)" | sed "s/\n//") + # Get the second to last octet + update_octet="${octets[2]}" + + if [ "$update_octet" -gt "$max" ]; then + max="$update_octet" + fi + + # Update Octet on last itr + if ((i == total_subnets - 1)); then + ((max++)) + updated_subnet="${octets[0]}.${octets[1]}.$max.${octets[3]}" + + # Reconstruct the updated CIDR block + updated_cidr_block="$updated_subnet/$subnet_mask" + SubnetCIDRBlock="$updated_cidr_block" + fi + done + fi + - # If the status returns a ROLLBACK or FAILED state at any point then launch has failed - if [ -n "${FAILED_RES}" ]; then - echo "Stack launch failed. Inspect AWS console for details. Remember to delete the stack after debugging!" + local RESULT=$(aws --region us-east-1 cloudformation create-stack --template-body "file://${CARMA_CLOUD_SIM_CLOUD_FORMATION_PATH}" \ + --stack-name "${STACK_NAME}" \ + --parameters ParameterKey=Cloud9AccessRoleName,ParameterValue="${USER_ARN}" \ + ParameterKey=Password,ParameterValue="${PASS}" \ + ParameterKey=CarmaConfigImage,ParameterValue="${CONFIG_IMAGE}" \ + ParameterKey=ExistingVPCID,ParameterValue="${VPC_ID}" \ + ParameterKey=ExistingRouteTableID,ParameterValue="${ROUTETABLE_ID}" \ + ParameterKey=EC2SubnetCIDRBlock,ParameterValue="${SubnetCIDRBlock}" \ + --capabilities CAPABILITY_NAMED_IAM) + + local VALID_LAUNCH=$(echo "${RESULT}" | grep 'arn:aws:cloudformation:us-east-1') + if [ -z "${VALID_LAUNCH}" ]; then + echo "AWS Launch Failed with error:" + echo ${RESULT} exit 1 fi - - TOTAL_STACK_RESOURCE=$(echo "${STACK_RESOURCE_STATUS}" | wc -l) - LAUNCHED_RESOURCE_COUNT=$(echo "${STACK_RESOURCE_STATUS}" | grep "CREATE_COMPLETE" | wc -l) + echo "AWS Stack ${STACK_NAME} starting up......." + + local STACK_RESOURCE_STATUS="NOT_EMPTY" + local LAUNCHED_RESOURCE_COUNT=0 + local PREV_LAUNCH_COUNT=0 + local i=0 + local SLEEP_SEC=5 + local LAUNCH_TIMEOUT=595 # 10m timeout to launch the stack + local ELAPSED_TIME=0 + local NON_COMPLETE_RES="ALL" + local FAILED_RES="NONE" + local TOTAL_STACK_RESOURCE=0 - if [ ${PREV_LAUNCH_COUNT} -ne ${LAUNCHED_RESOURCE_COUNT} ]; then - echo "Launched Stack Resource: ${LAUNCHED_RESOURCE_COUNT}/${TOTAL_STACK_RESOURCE} Elapsed Sec: ${ELAPSED_TIME}" + sleep ${SLEEP_SEC} + + # As long as there are resources which do not have the status CREATE_COMPLETE + # TODO: Ideally we wouldn't need to hard code 14 resources but a rare race condition can occur where 13 complete before 14 starts which leads to the need to hard code it for now + while { [ -n "${NON_COMPLETE_RES}" ] || [ ${TOTAL_STACK_RESOURCE} -lt 14 ]; } && [ ${ELAPSED_TIME} -lt ${LAUNCH_TIMEOUT} ]; do + + STACK_RESOURCE_STATUS=$(aws cloudformation describe-stack-resources --stack-name "${STACK_NAME}" | grep -oh '"ResourceStatus":.*",' | sed "s|\s*\"ResourceStatus\": \"||" | sed "s|\",||") + + NON_COMPLETE_RES=$(echo "${STACK_RESOURCE_STATUS}" | grep -v "CREATE_COMPLETE" | sed "s/\n//" ) + FAILED_RES=$(echo "${STACK_RESOURCE_STATUS}" | grep "\(ROLLBACK\|FAILED\|DELETE\)" | sed "s/\n//") + + # If the status returns a ROLLBACK or FAILED state at any point then launch has failed + if [ -n "${FAILED_RES}" ]; then + echo "Stack launch failed. Inspect AWS console for details. Remember to delete the stack after debugging!" + exit 1 + fi + + TOTAL_STACK_RESOURCE=$(echo "${STACK_RESOURCE_STATUS}" | wc -l) - PREV_LAUNCH_COUNT=${LAUNCHED_RESOURCE_COUNT} + LAUNCHED_RESOURCE_COUNT=$(echo "${STACK_RESOURCE_STATUS}" | grep "CREATE_COMPLETE" | wc -l) + + if [ ${PREV_LAUNCH_COUNT} -ne ${LAUNCHED_RESOURCE_COUNT} ]; then + echo "Launched Stack Resource: ${LAUNCHED_RESOURCE_COUNT}/${TOTAL_STACK_RESOURCE} Elapsed Sec: ${ELAPSED_TIME}" + + PREV_LAUNCH_COUNT=${LAUNCHED_RESOURCE_COUNT} - elif [ $(( ELAPSED_TIME % 20 )) == 0 ]; then - echo "Waiting on resources. Launched Stack Resources: ${LAUNCHED_RESOURCE_COUNT}/${TOTAL_STACK_RESOURCE} Elapsed Sec: ${ELAPSED_TIME}" - fi + elif [ $(( ELAPSED_TIME % 20 )) == 0 ]; then + echo "Waiting on resources. Launched Stack Resources: ${LAUNCHED_RESOURCE_COUNT}/${TOTAL_STACK_RESOURCE} Elapsed Sec: ${ELAPSED_TIME}" + fi - ((i++)) + ((i++)) - # Wait SLEEP_SEC seconds before checking again - sleep ${SLEEP_SEC} - ELAPSED_TIME=$((SLEEP_SEC * i)) + # Wait SLEEP_SEC seconds before checking again + sleep ${SLEEP_SEC} + ELAPSED_TIME=$((SLEEP_SEC * i)) - done + done - if [ ${ELAPSED_TIME} -gt ${LAUNCH_TIMEOUT} ]; then - echo "Stack launch timedout. Inspect AWS console for details. Remember to delete the stack after debugging!" - exit 1 - fi + if [ ${ELAPSED_TIME} -gt ${LAUNCH_TIMEOUT} ]; then + echo "Stack launch timedout. Inspect AWS console for details. Remember to delete the stack after debugging!" + exit 1 + fi - local DEV_MACHINE_ID=$(aws cloudformation describe-stack-resources --stack-name ${STACK_NAME} --output text --logical-resource-id DevMachine --query StackResources[].PhysicalResourceId) + local DEV_MACHINE_ID=$(aws cloudformation describe-stack-resources --stack-name ${STACK_NAME} --output text --logical-resource-id DevMachine --query StackResources[].PhysicalResourceId) - local PUBLIC_DNS=$(aws ec2 describe-instances --instance-ids ${DEV_MACHINE_ID} --output text --query Reservations[].Instances[].PublicDnsName) + local PUBLIC_DNS=$(aws ec2 describe-instances --instance-ids ${DEV_MACHINE_ID} --output text --query Reservations[].Instances[].PublicDnsName) - local CLOUD9_ADDRESS_BASE="https://us-east-1.console.aws.amazon.com/cloud9/ide" - # 1. Extract cloud9 name from stack details - local CLOUD9_ENVS=$(aws cloud9 list-environments --output text | sed "s|ENVIRONMENTIDS\s||") - # 2. Determine cloud9 environment id - local CLOUD9_NAME="RobotWorkshop-${STACK_NAME}" - local CLOUD9_ID=$(aws cloud9 describe-environments --output text --environment-ids $(echo $CLOUD9_ENVS) --query "environments[?name=='${CLOUD9_NAME}'].id") - # 3. Update URL - local CLOUD9_ADDRESS="${CLOUD9_ADDRESS_BASE}/${CLOUD9_ID}" + local CLOUD9_ADDRESS_BASE="https://us-east-1.console.aws.amazon.com/cloud9/ide" + # 1. Extract cloud9 name from stack details + local CLOUD9_ENVS=$(aws cloud9 list-environments --output text | sed "s|ENVIRONMENTIDS\s||") + # 2. Determine cloud9 environment id + local CLOUD9_NAME="RobotWorkshop-${STACK_NAME}" + local CLOUD9_ID=$(aws cloud9 describe-environments --output text --environment-ids $(echo $CLOUD9_ENVS) --query "environments[?name=='${CLOUD9_NAME}'].id") + # 3. Update URL + local CLOUD9_ADDRESS="${CLOUD9_ADDRESS_BASE}/${CLOUD9_ID}" - # Save the stack name for cleanup - echo "STACK_NAME=${STACK_NAME}" > ${CARMA_CLOUD_SIM_STACK_PATH} + # Save the stack name for cleanup + echo "STACK_NAME=${STACK_NAME}" > ${CARMA_CLOUD_SIM_STACK_PATH} + + # Print for user + echo "##############################" + echo "" + echo "AWS Stack ${STACK_NAME} Launch Suceeded" + echo "" + echo "EC2 Instance Address: ${PUBLIC_DNS}" + echo "" + echo "Cloud9 IDE Address: ${CLOUD9_ADDRESS}" + echo "" + echo "##############################" - # Print for user - echo "##############################" - echo "" - echo "AWS Stack ${STACK_NAME} Launch Suceeded" - echo "" - echo "EC2 Instance Address: ${PUBLIC_DNS}" - echo "" - echo "Cloud9 IDE Address: ${CLOUD9_ADDRESS}" - echo "" - echo "##############################" + else + echo -e "Required resources have not been setup. Please run \e[3m\e[1mcarma cloud-sim init-resources\e[0m first" + exit 1 + fi } cloud-sim__stop() { @@ -291,6 +352,213 @@ cloud-sim__status() { aws cloudformation describe-stack-resources --stack-name "${STACK_NAME}" } +# Function to create the VPC, internet gateway, and route table +cloud-sim__init-resources() { + + if [ ! command -v "aws" &> /dev/null ]; then + echo "aws cli is not installed. Attempting to install. This requires sudo." + + curl "https://awscli.amazonaws.com/awscli-exe-linux-x86_64.zip" -o "awscliv2.zip" + unzip awscliv2.zip + sudo ./aws/install + + echo "NOTE: You need to manually configure the aws cli for your user access key. Refer to this video for instructions https://www.youtube.com/watch?v=Rp-A84oh4G8" + exit 1 + fi + + # Check if resources already exist - reset if all don't exist + local vpc_success=0 + local igw_success=0 + local route_table_success=0 + + # Get resource status - query_resource_status defines VPC, Internet Gateway and Route Table ID + + if query_resource_status; then + echo -e "CDASim resources already exist. No change required. Run \e[3m\e[1mcarma cloud-sim start\e[0m to bring up stack. + VPC: ${vpc_id}, Internet Gateway: ${igw_id}, Route Table: ${route_table_id}" + exit 1 + + else + + # Try deleting CDASim resources if `all` don't exist + echo "Checking and deleting existing CDASim resources first" + cloud-sim__cleanup-resources + + + # Set desired CIDR block + local CIDR_Block="10.0.0.0/16" + + # Create VPC + vpc_id=$(aws ec2 create-vpc --cidr-block $CIDR_Block --tag-specifications 'ResourceType=vpc,Tags=[{Key=Name,Value=CDASim VPC}]' \ + --query 'Vpc.VpcId' --output text) + + aws ec2 modify-vpc-attribute --vpc-id $vpc_id --enable-dns-support + aws ec2 modify-vpc-attribute --vpc-id $vpc_id --enable-dns-hostnames + # Check the exit code to determine if the VPC creation was successful + if [ $? -eq 0 ]; then + vpc_success=1 + echo "Created vpc: $vpc_id" + else + echo "Error creating CDASim VPC. Configuration Failed, deleting resources allocated" + cloud-sim__cleanup-resources + fi + + # Create Internet Gateway + igw_id=$(aws ec2 create-internet-gateway \ + --tag-specifications 'ResourceType=internet-gateway,Tags=[{Key=Name,Value=CDASim IGW}]' --query 'InternetGateway.InternetGatewayId' \ + --output text) + + # Check the exit code to determine if the Internet Gateway creation was successful + if [ $? -eq 0 ]; then + igw_success=1 + echo "Created Internet Gateway: $igw_id" + else + echo "Error creating Internet Gateway. Configuration Failed, deleting resources allocated" + cloud-sim__cleanup-resources + fi + + # Attach Internet Gateway to VPC + aws ec2 attach-internet-gateway --internet-gateway-id $igw_id --vpc-id $vpc_id + + # Create Route Table + route_table_id=$(aws ec2 create-route-table --vpc-id $vpc_id \ + --tag-specifications 'ResourceType=route-table,Tags=[{Key=Name,Value=CDASim RouteTable}]' --query 'RouteTable.RouteTableId' \ + --output text) + + # Check the exit code to determine if the Route Table creation was successful + if [ $? -eq 0 ]; then + route_table_success=1 + echo "Created route table: $route_table_id" + else + echo "Error creating Route Table. Configuration Failed, deleting resources allocated" + cloud-sim__cleanup-resources + fi + + # Create a default route to the Internet Gateway + aws ec2 create-route --route-table-id $route_table_id --destination-cidr-block 0.0.0.0/0 --gateway-id $igw_id &> /dev/null + + if [ $? -eq 0 ]; then + echo "Default route created successfully!" + else + echo "Error creating default route to Internet Gateway. Configuration Failed, deleting resources allocated" + cloud-sim__cleanup-resources + fi + + + if [ $vpc_success -eq 1 ] && [ $igw_success -eq 1 ] && [ $route_table_success -eq 1 ]; then + echo "CDASim required resources were created successfully!" + else + echo "Some resources encountered errors. See above for details." + fi + fi + +} + +# Queries AWS for existing CDASim VPC, internet gateway, and route table and sets global variables +query_resource_status() { + # Get VPC ID based on tag name + vpc_id=$(aws ec2 describe-vpcs --filters "Name=tag:Name,Values=CDASim VPC" --query 'Vpcs[0].VpcId' --output text) + + # Get Internet Gateway ID based on tag name + igw_id=$(aws ec2 describe-internet-gateways --filters "Name=tag:Name,Values=CDASim IGW" \ + --query 'InternetGateways[0].InternetGatewayId' --output text) + + + # Get Route Table ID based on tag name + route_table_id=$(aws ec2 describe-route-tables --filters "Name=tag:Name,Values=CDASim RouteTable" \ + --query 'RouteTables[0].RouteTableId' --output text) + + local vpc_success=0 + local igw_success=0 + local route_table_success=0 + + if [ -z "$vpc_id" ] || [ $vpc_id != "None" ]; then + vpc_success=1 + fi + if [ -z "$igw_id" ] || [ $igw_id != "None" ]; then + igw_success=1 + fi + if [ -z "$igw_id" ] || [ $igw_id != "None" ]; then + route_table_success=1 + fi + + if [ $vpc_success -eq 1 ] && [ $igw_success -eq 1 ] && [ $route_table_success -eq 1 ]; then + + return 0 # All resources setup successfully + fi + return 1 # All resources not setup successfully + +} + +# Function to delete the VPC, internet gateway, and route table based on tag name +cloud-sim__cleanup-resources() { + + if [ ! command -v "aws" &> /dev/null ]; then + echo "aws cli is not installed. Attempting to install. This requires sudo." + + curl "https://awscli.amazonaws.com/awscli-exe-linux-x86_64.zip" -o "awscliv2.zip" + unzip awscliv2.zip + sudo ./aws/install + + echo "NOTE: You need to manually configure the aws cli for your user access key. Refer to this video for instructions https://www.youtube.com/watch?v=Rp-A84oh4G8" + exit 1 + fi + + echo "Trying to delete CDASim resources..." + query_resource_status + + local vpc_success=0 + local igw_success=0 + local route_table_success=0 + + + if [ -z "$route_table_id" ] || [ $route_table_id != "None" ]; then + # Delete the route table + aws ec2 delete-route-table --route-table-id $route_table_id + if [ $? -eq 0 ]; then + route_table_success=1 + echo "Deleted Route Table: $route_table_id" + else + echo "Failed to delete route table. Cleanup resources Failed" + exit 1 + fi + fi + + if [ -z "$igw_id" ] || [ $igw_id != "None" ]; then + # Detach and delete the Internet Gateway + aws ec2 detach-internet-gateway --internet-gateway-id $igw_id --vpc-id $vpc_id + aws ec2 delete-internet-gateway --internet-gateway-id $igw_id + if [ $? -eq 0 ]; then + igw_success=1 + echo "Deleted Internet Gateway: $igw_id" + else + echo "Failed to delete internet gateway. Cleanup resources Failed" + exit 1 + fi + + fi + + if [ -z "$vpc_id" ] || [ $vpc_id != "None" ]; then + # Delete the VPC + aws ec2 delete-vpc --vpc-id $vpc_id + if [ $? -eq 0 ]; then + vpc_success=1 + echo "Deleted VPC: $vpc_id" + else + echo "Failed to delete VPC. Cleanup resources Failed" + exit 1 + fi + + fi + + if [ $vpc_success -eq 1 ] || [ $igw_success -eq 1 ] || [ $route_table_success -eq 1 ]; then + echo "CDASim Resources were deleted successfully!" + else + echo "No resources required to be deleted." + fi + +} + cloud-sim__help() { cat < -#include -#include "entry_manager.h" -#include - -namespace health_monitor -{ - class DriverManager - { - public: - - /*! - * \brief Default constructor for DriverManager with driver_timeout_ = 1000 - */ - DriverManager(); - /*! - * \brief Constructor for DriverManager takes in crtitical driver names and driver timeout - */ - DriverManager(std::vector critical_driver_names, const long driver_timeout,std::vector lidar_gps_driver_names, - std::vector camera_driver_names); - /*! - * \brief Update driver status - */ - void update_driver_status(const cav_msgs::DriverStatusConstPtr& msg, long current_time); - /*! - * \brief Check if all critical drivers are operational for truck - */ - std::string are_critical_drivers_operational_truck(long current_time); - /*! - * \brief Check if all critical drivers are operational for car - */ - std::string are_critical_drivers_operational_car(long current_time); - /*! - * \brief Evaluate if the sensor is available - */ - void evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout); - /*! - * \brief Handle the spin and publisher - */ - cav_msgs::SystemAlert handleSpin(bool truck,bool car,long time_now,long start_up_timestamp,long startup_duration); - - private: - - EntryManager em_; - bool starting_up_ = true; - // timeout for critical driver timeout - long driver_timeout_ {1000}; - - }; -} \ No newline at end of file diff --git a/health_monitor/include/entry_manager.h b/health_monitor/include/entry_manager.h deleted file mode 100644 index 13b559dbfa..0000000000 --- a/health_monitor/include/entry_manager.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2019-2021 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 -#include -#include "entry.h" - -namespace health_monitor -{ - class EntryManager - { - public: - - /*! - * \brief Default constructor for EntryManager. - */ - EntryManager(); - /*! - * \brief Constructor for EntryManager to set required entries. - */ - EntryManager(std::vector required_entries); - - /*! - * \brief Constructor for EntryManager to set required entries and lidar gps entires. - */ - EntryManager(std::vector required_entries,std::vector lidar_gps_entries, - std::vector camera_entries); - - /*! - * \brief Add a new entry if the given name does not exist. - * Update an existing entry if the given name exists. - */ - void update_entry(const Entry& entry); - - /*! - * \brief Get all registed entries as a list. - */ - std::vector get_entries() const; - - /*! - * \brief Get a entry using name as the key. - */ - boost::optional get_entry_by_name(const std::string& name) const; - - /*! - * \brief Delete an entry using the given name as the key. - */ - void delete_entry(const std::string& name); - - /*! - * \brief Check if the entry is required - */ - bool is_entry_required(const std::string& name) const; - /*! - * \brief Check if the entry is required - */ - int is_lidar_gps_entry_required(const std::string& name) const; - - int is_camera_entry_required(const std::string& name) const; - - private: - - // private list to keep track of all entries - std::vector entry_list_; - - // list of required entries - std::vector required_entries_; - - // list of lidar and gps entries - std::vector lidar_gps_entries_; - - std::vector camera_entries_; - - }; -} \ No newline at end of file diff --git a/health_monitor/include/health_monitor.h b/health_monitor/include/health_monitor.h deleted file mode 100644 index 79d3f64511..0000000000 --- a/health_monitor/include/health_monitor.h +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2019-2021 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 -#include -#include -#include -#include -#include -#include "driver_manager.h" - -namespace health_monitor -{ - class HealthMonitor - { - public: - - /*! - * \brief Default constructor for HealthMonitor - */ - HealthMonitor(); - /*! - * \brief Begin normal execution of health monitor node. Will take over control flow of program and exit from here. - * - * \return The exit status of this program - */ - void run(); - - // spin callback function - bool spin_cb(); - - - - //Unit Testing Functions - void setDriverManager(DriverManager dm); - void setCarTrue(); - void setTruckTrue(); - - private: - - // node handles - std::shared_ptr nh_; - std::shared_ptr pnh_; - - // workers - DriverManager driver_manager_; - - // topic subscribers - ros::Subscriber driver_discovery_subscriber_; - - // message/service callbacks - void driver_discovery_cb(const cav_msgs::DriverStatusConstPtr& msg); - - // initialize method - void initialize(); - - // ROS params - double spin_rate_, driver_timeout_, startup_duration_; - std::vector required_drivers_; - std::vector lidar_gps_drivers_; - std::vector camera_drivers_; - - bool truck_; - bool car_; - - // record of startup timestamp - long start_up_timestamp_; - - // Previously published alert message - boost::optional prev_alert; - }; -} \ No newline at end of file diff --git a/health_monitor/launch/health_monitor.launch b/health_monitor/launch/health_monitor.launch deleted file mode 100644 index 1dde8a4ba1..0000000000 --- a/health_monitor/launch/health_monitor.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/health_monitor/package.xml b/health_monitor/package.xml deleted file mode 100644 index 407904863b..0000000000 --- a/health_monitor/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - health_monitor - 3.3.0 - The node is used to manage and monitor plugin and driver status - qiangs - Apache 2.0 License - catkin - carma_utils - cav_msgs - cav_srvs - roscpp - std_msgs - carma_cmake_common - diff --git a/health_monitor/src/driver_manager.cpp b/health_monitor/src/driver_manager.cpp deleted file mode 100644 index b823ccd429..0000000000 --- a/health_monitor/src/driver_manager.cpp +++ /dev/null @@ -1,355 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 "driver_manager.h" -namespace health_monitor -{ - - DriverManager::DriverManager() {} - - DriverManager::DriverManager(std::vector critical_driver_names, const long driver_timeout, std::vector lidar_gps_driver_names, - std::vector camera_driver_names): - em_(EntryManager(critical_driver_names,lidar_gps_driver_names, camera_driver_names)), driver_timeout_(driver_timeout) {} - - void DriverManager::update_driver_status(const cav_msgs::DriverStatusConstPtr& msg, long current_time) - { - Entry driver_status(msg->status == cav_msgs::DriverStatus::OPERATIONAL || msg->status == cav_msgs::DriverStatus::DEGRADED,true, msg->name, current_time, 0, ""); - em_.update_entry(driver_status); - // NOTE: The following is a temporary hack to allow the lidar driver to be moved to ROS2 which will not use this node - Entry fake_entry(msg->status == cav_msgs::DriverStatus::OPERATIONAL,true, msg->name, current_time, 0, ""); - } - - void DriverManager::evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout) - { - if((!available) || (current_time-timestamp > driver_timeout)) - { - sensor_input=0; - } - else - { - sensor_input=1; - } - } - - std::string DriverManager::are_critical_drivers_operational_truck(long current_time) - { - int ssc=0; - int lidar1=0; - int lidar2=0; - int gps=0; - int camera=0; //Add Camera Driver - - std::vector driver_list = em_.get_entries(); //Real time driver list from driver status - for(auto i = driver_list.begin(); i < driver_list.end(); ++i) - { - if(em_.is_entry_required(i->name_)) - { - evaluate_sensor(ssc,i->available_,current_time,i->timestamp_,driver_timeout_); - } - - if(em_.is_lidar_gps_entry_required(i->name_)==0) //Lidar1 - { - evaluate_sensor(lidar1,i->available_,current_time,i->timestamp_,driver_timeout_); - } - else if(em_.is_lidar_gps_entry_required(i->name_)==1) //Lidar2 - { - evaluate_sensor(lidar2,i->available_,current_time,i->timestamp_,driver_timeout_); - } - else if(em_.is_lidar_gps_entry_required(i->name_)==2) //GPS - { - evaluate_sensor(gps,i->available_,current_time,i->timestamp_,driver_timeout_); - } - else if(em_.is_camera_entry_required(i->name_)==0) - { - evaluate_sensor(camera,i->available_,current_time,i->timestamp_,driver_timeout_); - } - - } - - ////////////////////// - // NOTE: THIS IS A MANUAL DISABLE OF ALL LIDAR AND GPS FAILURE DETECTION FOLLOWING THE ROS2 PORT - ///////////////////// - lidar1=1; - lidar2=1; - gps=1; - ///////////////////// - - //Decision making - if (ssc == 0) - { - return "s_0"; - } - // if ssc= 1 - if((lidar1==0) && (lidar2==0) && (gps==0)) - { - return "s_1_l1_0_l2_0_g_0"; - } - else if((lidar1==0) && (lidar2==0) && (gps==1)) - { - return "s_1_l1_0_l2_0_g_1"; - } - else if((lidar1==0) && (lidar2==1) && (gps==0)) - { - return "s_1_l1_0_l2_1_g_0"; - } - else if((lidar1==0) && (lidar2==1) && (gps==1)) - { - return "s_1_l1_0_l2_1_g_1"; - } - else if((lidar1==1) && (lidar2==0) && (gps==0)) - { - return "s_1_l1_1_l2_0_g_0"; - } - else if((lidar1==1) && (lidar2==0) && (gps==1)) - { - return "s_1_l1_1_l2_0_g_1"; - } - else if((lidar1==1) && (lidar2==1) && (gps==0)) - { - return "s_1_l1_1_l2_1_g_0"; - } - else if ((camera==0) && (lidar1 ==1) && (lidar2 == 1) && (gps == 1) ) - { - return "s_1_l1_1_l2_1_g_1_c_0"; - } - else if((lidar1==1) && (lidar2==1) && (gps==1) && (camera == 1)) - { - return "s_1_l1_1_l2_1_g_1_c_1"; - } - - } - - - std::string DriverManager::are_critical_drivers_operational_car(long current_time) - { - int ssc=0; - int lidar=0; - int gps=0; - int camera=0; - std::vector driver_list = em_.get_entries(); //Real time driver list from driver status - for(auto i = driver_list.begin(); i < driver_list.end(); ++i) - { - if(em_.is_entry_required(i->name_)) - { - evaluate_sensor(ssc,i->available_,current_time,i->timestamp_,driver_timeout_); - } - if(em_.is_lidar_gps_entry_required(i->name_)==0) //Lidar - { - evaluate_sensor(lidar,i->available_,current_time,i->timestamp_,driver_timeout_); - } - else if(em_.is_lidar_gps_entry_required(i->name_)==1) //GPS - { - evaluate_sensor(gps,i->available_,current_time,i->timestamp_,driver_timeout_); - } - else if(em_.is_camera_entry_required(i->name_)==0) - { - evaluate_sensor(camera,i->available_,current_time,i->timestamp_,driver_timeout_); - } - } - - ////////////////////// - // NOTE: THIS IS A MANUAL DISABLE OF ALL LIDAR FAILURE DETECTION FOLLOWING THE ROS2 PORT - ///////////////////// - lidar=1; - gps=1; - ///////////////////// - - //Decision making - if(ssc==1) - { - if((lidar==0) && (gps==0)) - { - return "s_1_l_0_g_0"; - } - - else if((lidar==0) && (gps==1) && (camera == 1)) - { - return "s_1_l_0_g_1"; - } - else if((lidar==1) && (gps==0) && (camera == 1)) - { - return "s_1_l_1_g_0"; - } - else if ((camera==0) && (lidar == 1) && (gps ==1)) - { - return "s_1_l_1_g_1_c_0"; - } - else if ((camera==0) && (lidar == 0) && (gps == 1)) - { - return "s_1_l_0_g_1_c_0"; - } - else if ((camera==0) && (lidar == 1) && (gps == 0)) - { - return "s_1_l_1_g_0_c_0"; - } - else if((lidar==1) && (gps==1) && (camera == 1)) - { - return "s_1_l_1_g_1_c_1"; - } - - } - else - { - return "s_0"; - } - - } - - cav_msgs::SystemAlert DriverManager::handleSpin(bool truck,bool car,long time_now,long start_up_timestamp,long start_duration) - { - cav_msgs::SystemAlert alert; - - if(truck==true) - { - std::string status = are_critical_drivers_operational_truck(time_now); - if(status.compare("s_1_l1_1_l2_1_g_1_c_1") == 0) - { - starting_up_ = false; - alert.description = "All essential drivers are ready"; - alert.type = cav_msgs::SystemAlert::DRIVERS_READY; - return alert; - } - else if(starting_up_ && (time_now - start_up_timestamp <= start_duration)) - { - alert.description = "System is starting up..."; - alert.type = cav_msgs::SystemAlert::NOT_READY; - return alert; - } - else if(status.compare("s_1_l1_1_l2_1_g_1_c_0")==0) - { - alert.description = "Camera Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - } - else if((status.compare("s_1_l1_0_l2_1_g_1") == 0) || (status.compare("s_1_l1_1_l2_0_g_1") == 0)) - { - - alert.description = "One LIDAR Failed"; - alert.type = cav_msgs::SystemAlert::CAUTION; - return alert; - } - else if((status.compare("s_1_l1_0_l2_1_g_0") == 0) || (status.compare("s_1_l1_1_l2_0_g_0") == 0)) - { - alert.description = "One Lidar and GPS Failed"; - alert.type = cav_msgs::SystemAlert::CAUTION; - return alert; - } - else if(status.compare("s_1_l1_1_l2_1_g_0") == 0) - { - alert.description = "GPS Failed"; - alert.type = cav_msgs::SystemAlert::CAUTION; - return alert; - } - else if(status.compare("s_1_l1_0_l2_0_g_1") == 0) - { - alert.description = "Both LIDARS Failed"; - alert.type = cav_msgs::SystemAlert::WARNING; - return alert; - } - else if(status.compare("s_1_l1_0_l2_0_g_0") == 0) - { - alert.description = "LIDARS and GPS Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else if(status.compare("s_0") == 0) - { - alert.description = "SSC Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else - { - alert.description = "Unknown problem assessing essential driver availability"; - alert.type = cav_msgs::SystemAlert::FATAL; - return alert; - } - - } - else if(car==true) - { - std::string status = are_critical_drivers_operational_car(time_now); - if(status.compare("s_1_l_1_g_1_c_1") == 0) - { - starting_up_ = false; - alert.description = "All essential drivers are ready"; - alert.type = cav_msgs::SystemAlert::DRIVERS_READY; - return alert; - } - else if(starting_up_ && (time_now - start_up_timestamp <= start_duration)) - { - alert.description = "System is starting up..."; - alert.type = cav_msgs::SystemAlert::NOT_READY; - return alert; - } - - else if(status.compare("s_1_l_1_g_1_c_0") == 0) - { - alert.description = "Camera Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else if(status.compare("s_1_l_1_g_0") == 0) - { - alert.description = "GPS Failed"; - alert.type = cav_msgs::SystemAlert::CAUTION; - return alert; - } - else if(status.compare("s_1_l_0_g_1") == 0) - { - alert.description = "LIDAR Failed"; - alert.type = cav_msgs::SystemAlert::WARNING; - return alert; - } - else if(status.compare("s_1_l_0_g_0") == 0) - { - alert.description = "LIDAR, GPS Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else if(status.compare("s_1_l_0_g_1_c_0") == 0) - { - alert.description = "LIDAR, Camera Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else if(status.compare("s_1_l_1_g_0_c_0") == 0) - { - alert.description = " GPS, Camera Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else if(status.compare("s_0") == 0) - { - alert.description = "SSC Failed"; - alert.type = cav_msgs::SystemAlert::SHUTDOWN; - return alert; - } - else - { - alert.description = "Unknown problem assessing essential driver availability"; - alert.type = cav_msgs::SystemAlert::FATAL; - return alert; - } - } - else - { - alert.description = "Need to set either truck or car flag"; - alert.type = cav_msgs::SystemAlert::FATAL; - return alert; - } - } - -} diff --git a/health_monitor/src/entry.cpp b/health_monitor/src/entry.cpp deleted file mode 100644 index dde9817d7d..0000000000 --- a/health_monitor/src/entry.cpp +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 "entry.h" - -namespace health_monitor -{ - Entry::Entry(bool available, bool active, const std::string& name, long timestamp, uint8_t type, const std::string& capability) : - available_(available), active_(active), name_(name), timestamp_(timestamp), type_(type), capability_(capability) {} - -} \ No newline at end of file diff --git a/health_monitor/src/health_monitor.cpp b/health_monitor/src/health_monitor.cpp deleted file mode 100644 index 37ad46debd..0000000000 --- a/health_monitor/src/health_monitor.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 "health_monitor.h" -#include -#include "carma_utils/CARMANodeHandle.h" - - -namespace health_monitor -{ - - HealthMonitor::HealthMonitor() - { - car_ = false; - truck_ = false; - } - void HealthMonitor::initialize() - { - // init node handles - ROS_INFO_STREAM("Initalizing health_monitor node..."); - pnh_.reset(new ros::CARMANodeHandle("~")); - nh_.reset(new ros::CARMANodeHandle()); - // init ros service servers, publishers and subscribers - driver_discovery_subscriber_ = nh_->subscribe("driver_discovery", 5, &HealthMonitor::driver_discovery_cb, this); - - // load params - spin_rate_ = pnh_->param("spin_rate_hz", 10.0); - driver_timeout_ = pnh_->param("required_driver_timeout", 500); - startup_duration_ = pnh_->param("startup_duration", 25); - - pnh_->getParam("required_drivers", required_drivers_); - pnh_->getParam("lidar_gps_drivers", lidar_gps_drivers_); - pnh_->getParam("camera_drivers",camera_drivers_); - - truck_=false; - car_=false; - pnh_->getParam("truck", truck_); - pnh_->getParam("car", car_); - - // Log parameters - ROS_INFO_STREAM("Health Monitor Parameters {"); - ROS_INFO_STREAM("spin_rate_hz: " << spin_rate_); - ROS_INFO_STREAM("required_driver_timeout: " << driver_timeout_); - ROS_INFO_STREAM("startup_duration: " << startup_duration_); - ROS_INFO_STREAM("truck: " << truck_); - ROS_INFO_STREAM("car: " << car_); - - ROS_INFO_STREAM("required_drivers: ["); - for(auto p : required_drivers_) { - ROS_INFO_STREAM(" " << p); - } - ROS_INFO_STREAM(" ]"); - - ROS_INFO_STREAM("lidar_gps_drivers: ["); - for(auto p : lidar_gps_drivers_) { - ROS_INFO_STREAM(" " << p); - } - ROS_INFO_STREAM(" ]"); - - ROS_INFO_STREAM("camera_drivers: ["); - for(auto p : camera_drivers_) { - ROS_INFO_STREAM(" " << p); - } - - ROS_INFO_STREAM(" ]"); - ROS_INFO_STREAM("}"); - - - - // initialize worker class - driver_manager_ = DriverManager(required_drivers_, driver_timeout_,lidar_gps_drivers_,camera_drivers_); - - // record starup time - start_up_timestamp_ = ros::Time::now().toNSec() / 1e6; - - ros::CARMANodeHandle::setSystemAlertCallback([&](const cav_msgs::SystemAlertConstPtr& msg) -> void { - - bool shutdown = false; - - - if (msg->type == cav_msgs::SystemAlert::FATAL) - { - // An empty source_node indicates that the alert was generated by a ROS1 node - // Therefore there is no complex error handling and the system should shutdown - if(msg->source_node.empty()) - { - shutdown = true; - } - // If the ROS2 system controller failed we need to shutdown - else if (msg->source_node.compare("/system_controller") == 0) - { - shutdown = true; - } - else { // No need to shutdown if the alert was generated by a ROS2 node and it wasn't the system controller - shutdown = false; - } - } - - - if (shutdown) - { - ROS_ERROR_STREAM("Health Monitor shutting down due to system alert: " << msg->description); - std::string header = "health_monitor requesting shutdown due to: " + msg->description; - - cav_msgs::SystemAlert new_msg; - new_msg.description = header; - new_msg.type = cav_msgs::SystemAlert::SHUTDOWN; - - nh_->publishSystemAlert(new_msg); - } - - }); - - - ros::CARMANodeHandle::setExceptionCallback([&](const std::exception& exp) -> void { - - cav_msgs::SystemAlert new_msg; - new_msg.type = cav_msgs::SystemAlert::SHUTDOWN; - new_msg.description = exp.what(); - - nh_->publishSystemAlert(new_msg); - - }); - - } - - void HealthMonitor::run() - { - initialize(); - ros::CARMANodeHandle::setSpinRate(spin_rate_); - ros::CARMANodeHandle::setSpinCallback(std::bind(&HealthMonitor::spin_cb, this)); - ros::CARMANodeHandle::spin(); - } - - - void HealthMonitor::driver_discovery_cb(const cav_msgs::DriverStatusConstPtr& msg) - { - // convert ros nanosecond to millisecond by the factor of 1/1e6 - driver_manager_.update_driver_status(msg, ros::Time::now().toNSec() / 1e6); - } - - bool HealthMonitor::spin_cb() - { - long time_now=(ros::Time::now().toNSec() / 1e6); - ros::Duration sd(startup_duration_); - long start_duration=sd.toNSec() / 1e6; - - auto dm = driver_manager_.handleSpin(truck_,car_,time_now,start_up_timestamp_,start_duration); - if (!prev_alert) { - prev_alert = dm; - nh_->publishSystemAlert(dm); - } else if (prev_alert->type == dm.type && prev_alert->description.compare(dm.description) == 0) { // Do not publish duplicate alerts - ROS_DEBUG_STREAM("No change to alert status"); - } else { - prev_alert = dm; - nh_->publishSystemAlert(dm); - } - - return true; - } - - - void HealthMonitor::setDriverManager(DriverManager dm) - { - driver_manager_ = dm; - } - - void HealthMonitor::setCarTrue() - { - car_ = true; - if(truck_ == true) - throw std::invalid_argument("truck_ = true"); - } - - void HealthMonitor::setTruckTrue() - { - truck_ = true; - if(car_ == true) - throw std::invalid_argument("car_ = true"); - - } - - -} diff --git a/health_monitor/src/main.cpp b/health_monitor/src/main.cpp deleted file mode 100644 index d5e0545920..0000000000 --- a/health_monitor/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 -#include "health_monitor.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "health_monitor"); - health_monitor::HealthMonitor node; - node.run(); - return 0; -}; diff --git a/health_monitor/test/test_driver_manager.cpp b/health_monitor/test/test_driver_manager.cpp deleted file mode 100644 index f50d52fb3c..0000000000 --- a/health_monitor/test/test_driver_manager.cpp +++ /dev/null @@ -1,1803 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 "driver_manager.h" -#include - -namespace health_monitor -{ - -//////////////////// Unit test for car part/////////////////////////////////////////////// - - TEST(DriverManagerTest, testCarNormalDriverStatus) - { - // inordinary case where no critical drivers are specified - DriverManager dm0; - cav_msgs::DriverStatus msg0; - msg0.controller = true; - msg0.name = "controller"; - msg0.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg0_pointer(new cav_msgs::DriverStatus(msg0)); - dm0.update_driver_status(msg0_pointer, 1000); - EXPECT_EQ("s_0", dm0.are_critical_drivers_operational_car(1500)); - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers, camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_1_g_1_c_1", dm.are_critical_drivers_operational_car(1500)); - } - - TEST(DriverManagerTest, testCarSsc_1_Lidar_0_Gps_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_0_g_0", dm.are_critical_drivers_operational_car(1500)); - } - - TEST(DriverManagerTest, testCarSsc_0_Lidar_1_Gps_1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational_car(1500)); - } - - TEST(DriverManagerTest, testCarSsc_1_Lidar_0_Gps_1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_0_g_1", dm.are_critical_drivers_operational_car(1500)); - } - - TEST(DriverManagerTest, testCarSsc_1_Lidar_1_Gps_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_1_g_0", dm.are_critical_drivers_operational_car(1500)); - } - - TEST(DriverManagerTest, testCarErrorDriverStatusTimeOut) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational_car(2100)); - } - ////////////////////////////////////////////////////////////////////////////////////////// - - TEST(DriverManagerTest, testCarHandleSpinDriversReady) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(5, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinCautionGpsNotWorkingLidarWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(1, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinWarningLidarNotWorkingGpsWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(2, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinFatalLidarNotWorkingGpsNotWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinFatalSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinFatalUnknownInside) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinNotReadyCase1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,1000,750); - - EXPECT_EQ(4, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinNotReadyCase2) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=true; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,5000,750); - - EXPECT_EQ(4, alert.type); - } - - - TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - bool truck=false; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(3, alert.type); - } - - -////////////////////////// Unit test for truck part//////////////////////////////////////// - - TEST(DriverManagerTest, testTruckNormalDriverStatus) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_1_g_1_c_1", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_0_Lidar1_1_Lidar2_1_Gps_1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational_truck(1500)); - } - - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_0_Lidar2_0_Gps_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_0_g_0", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_0_Lidar2_0_Gps_1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_0_g_1", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_0_Lidar2_1_Gps_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_1_g_0", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_0_Lidar2_1_Gps_1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_1_g_1", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_1_Lidar2_0_Gps_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_0_g_0", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_1_Lidar2_0_Gps_1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_0_g_1", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_1_Lidar2_1_Gps_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_1_g_0", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckSsc_1_Lidar1_1_Lidar2_1_Gps_1_Camera_0) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_1_g_1_c_0", dm.are_critical_drivers_operational_truck(1500)); - } - - TEST(DriverManagerTest, testTruckDriverStatusTimeout) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational_truck(2001)); - } -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - TEST(DriverManagerTest, testHandleSpinDriverReady) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(5, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinCautionOneLidar1WorkingGpsWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(1, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinCautionLidar1WorkingGpsNotWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(1, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinCautionOneLidar2WorkingGpsWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(1, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinCautionOneLidar1WorkingGpsNotWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(1, alert.type); - } - - - TEST(DriverManagerTest, testHandleSpinCautionOneLidar2WorkingGpsNotWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(1, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinWarningLidarNotWorkingGpsWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(2, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinFatalLidarNotWorkingGpsNotWorkingSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinFatalSscNotWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinFatalUnknownInsideTruck) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - - TEST(DriverManagerTest, testHandleSpinNotReadyCase1) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,1000,750); - - EXPECT_EQ(4, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinNotReadyCase2) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,5000,750); - - EXPECT_EQ(4, alert.type); - } - - TEST(DriverManagerTest, testHandleSpinWarningLidarWorkingGpsWorkingSscWorkingCameraNotWorking) - { - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - bool truck=true; - bool car=false; - - cav_msgs::SystemAlert alert; - alert=dm.handleSpin(truck,car,1500,150,750); - - EXPECT_EQ(6, alert.type); - } - -} \ No newline at end of file diff --git a/health_monitor/test/test_health_monitor.cpp b/health_monitor/test/test_health_monitor.cpp deleted file mode 100644 index 07e01a86a7..0000000000 --- a/health_monitor/test/test_health_monitor.cpp +++ /dev/null @@ -1,587 +0,0 @@ -#include "health_monitor.h" -#include "driver_manager.h" -#include -#include -#include - -namespace health_monitor -{ - - - TEST(HealthMonitorTest, runTest_s_1_l_1_g_1) - { - - HealthMonitor node; - /*Test the published status of each system alert*/ - - // inordinary case where no critical drivers are specified - DriverManager dm0; - cav_msgs::DriverStatus msg0; - msg0.controller = true; - msg0.name = "controller"; - msg0.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg0_pointer(new cav_msgs::DriverStatus(msg0)); - dm0.update_driver_status(msg0_pointer, 1000); - EXPECT_EQ("s_0", dm0.are_critical_drivers_operational_car(1500)); - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_1_g_1_c_1", dm.are_critical_drivers_operational_car(1500)); - -} - - TEST(HealthMonitorTest, runTest_s_1_l_1_g_0) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_1_g_0", dm.are_critical_drivers_operational_car(1500)); - - } - - - - TEST(HealthMonitorTest, runTest_s_1_l_0_g_1) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_0_g_1", dm.are_critical_drivers_operational_car(1500)); - - } - - - - TEST(HealthMonitorTest, runTest_s_1_l_0_g_0) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_1_l_0_g_0", dm.are_critical_drivers_operational_car(1500)); - - } - - TEST(HealthMonitorTest, runTest_s_0) - { - HealthMonitor node; - - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.gnss = true; - msg3.name = "gps"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "camera"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational_car(1500)); - - } - - TEST(HealthMonitorTest, runTest_s_1_l1_1_l2_1_g_1) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::DEGRADED; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_1_g_1_c_1", dm.are_critical_drivers_operational_truck(1500)); - - } - - TEST(HealthMonitorTest, runTest_s_1_l1_0_l2_1_g_1) - { - HealthMonitor node; - - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_1_g_1", dm.are_critical_drivers_operational_truck(1500)); - - } - - TEST(HealthMonitorTest, runTest_s_1_l1_0_l2_1_g_0) - { - HealthMonitor node; - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_1_g_0", dm.are_critical_drivers_operational_truck(1500)); - - } - - TEST(HealthMonitorTest, runTest_s_1_l1_1_l2_1_g_0) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_1_l2_1_g_0", dm.are_critical_drivers_operational_truck(1500)); - - - } - - TEST(HealthMonitorTest, runTest_s_1_l1_0_l2_0_g_1) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_0_g_1", dm.are_critical_drivers_operational_truck(1500)); - - } - - TEST(HealthMonitorTest, runTest_s_1_l1_0_l2_0_g_0) - { - HealthMonitor node; - - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_1_l1_0_l2_0_g_0", dm.are_critical_drivers_operational_truck(1500)); - - } - - TEST(HealthMonitorTest, runTest_truck_s_0) - { - HealthMonitor node; - - std::vector required_drivers{"controller"}; - std::vector lidar_gps_drivers{"lidar1", "lidar2","gps"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, 1000L,lidar_gps_drivers,camera_drivers); - - cav_msgs::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = cav_msgs::DriverStatus::OFF; - cav_msgs::DriverStatusConstPtr msg1_pointer(new cav_msgs::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - cav_msgs::DriverStatus msg2; - msg2.lidar = true; - msg2.name = "lidar1"; - msg2.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg2_pointer(new cav_msgs::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - cav_msgs::DriverStatus msg3; - msg3.lidar = true; - msg3.name = "lidar2"; - msg3.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg3_pointer(new cav_msgs::DriverStatus(msg3)); - dm.update_driver_status(msg3_pointer, 1000); - - cav_msgs::DriverStatus msg4; - msg4.gnss = true; - msg4.name = "gps"; - msg4.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg4_pointer(new cav_msgs::DriverStatus(msg4)); - dm.update_driver_status(msg4_pointer, 1000); - - cav_msgs::DriverStatus msg5; - msg5.gnss = true; - msg5.name = "camera"; - msg5.status = cav_msgs::DriverStatus::OPERATIONAL; - cav_msgs::DriverStatusConstPtr msg5_pointer(new cav_msgs::DriverStatus(msg5)); - dm.update_driver_status(msg5_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational_truck(1500)); - - - } - - - - -} \ No newline at end of file diff --git a/health_monitor/test/test_main.cpp b/health_monitor/test/test_main.cpp deleted file mode 100644 index a68575236b..0000000000 --- a/health_monitor/test/test_main.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 - -// Run all the tests -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/inlanecruising_plugin/config/parameters.yaml b/inlanecruising_plugin/config/parameters.yaml index 16f0a1e976..e4183d2647 100644 --- a/inlanecruising_plugin/config/parameters.yaml +++ b/inlanecruising_plugin/config/parameters.yaml @@ -9,5 +9,5 @@ curvature_moving_average_window_size: 9 # Size of the window used in the moving back_distance: 20.0 # Number of meters behind the first maneuver that need to be included in points for curvature calculation max_accel_multiplier: 0.85 # Multiplier of max_accel to bring the value under max_accel lat_accel_multiplier: 0.50 # Multiplier of lat_accel to bring the value under lat_accel TODO needs to be optimized -enable_object_avoidance: false # Activates object avoidance logic +enable_object_avoidance: true # Activates object avoidance logic buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points \ No newline at end of file diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index 97a708e3b9..3b2c83e347 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -106,8 +106,9 @@ void InLaneCruisingPlugin::plan_trajectory_callback( for (auto& p : original_trajectory.trajectory_points) { p.planner_plugin_name = plugin_name_; } - - if (config_.enable_object_avoidance) + + // Aside from the flag, ILC should not call yield_plugin on invalid trajectories + if (config_.enable_object_avoidance && original_trajectory.trajectory_points.size() >= 2) { RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Activate Object Avoidance"); diff --git a/intersection_transit_maneuvering/include/intersection_transit_maneuvering/call_interface.hpp b/intersection_transit_maneuvering/include/intersection_transit_maneuvering/call_interface.hpp index 45082f9a8d..c2d1a9e4ae 100644 --- a/intersection_transit_maneuvering/include/intersection_transit_maneuvering/call_interface.hpp +++ b/intersection_transit_maneuvering/include/intersection_transit_maneuvering/call_interface.hpp @@ -23,7 +23,7 @@ class CallInterface { public: - virtual void call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) = 0; + virtual void call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp) = 0; virtual ~CallInterface() {}; /** diff --git a/intersection_transit_maneuvering/include/intersection_transit_maneuvering/intersection_transit_maneuvering_node.hpp b/intersection_transit_maneuvering/include/intersection_transit_maneuvering/intersection_transit_maneuvering_node.hpp index 61d35c2c76..1358d8d52f 100644 --- a/intersection_transit_maneuvering/include/intersection_transit_maneuvering/intersection_transit_maneuvering_node.hpp +++ b/intersection_transit_maneuvering/include/intersection_transit_maneuvering/intersection_transit_maneuvering_node.hpp @@ -41,6 +41,7 @@ class IntersectionTransitManeuveringNode : public carma_guidance_plugins::Tactic public: std::shared_ptr object_; + carma_ros2_utils::ClientPtr client_; /** * \brief IntersectionTransitManeuveringNode constructor */ diff --git a/intersection_transit_maneuvering/include/intersection_transit_maneuvering/itm_service.hpp b/intersection_transit_maneuvering/include/intersection_transit_maneuvering/itm_service.hpp index a8f7ed5bd7..3649bad2f9 100644 --- a/intersection_transit_maneuvering/include/intersection_transit_maneuvering/itm_service.hpp +++ b/intersection_transit_maneuvering/include/intersection_transit_maneuvering/itm_service.hpp @@ -45,7 +45,7 @@ class Servicer: public CallInterface * @param resp Incoming PlanTrajectory service response * @return true if successful, otherwise false */ - void call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); + void call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp); /** * \brief set the trajectory service client diff --git a/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp b/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp index c3a219d033..790a98af0a 100644 --- a/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp +++ b/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp @@ -139,9 +139,9 @@ carma_ros2_utils::CallbackReturn IntersectionTransitManeuveringNode::on_configur object_ = std::make_shared(); - auto trajectory_client = create_client("inlanecruising_plugin/plan_trajectory"); + client_ = create_client("inlanecruising_plugin/plan_trajectory"); - object_->set_client(trajectory_client); + object_->set_client(client_); //Return success if everthing initialized successfully return CallbackReturn::SUCCESS; @@ -184,9 +184,9 @@ return CallbackReturn::SUCCESS; related_maneuvers.push_back(i); } else - { - break; - } + { + break; + } } converted_maneuvers_ = convert_maneuver_plan(maneuver_plan); @@ -197,17 +197,19 @@ return CallbackReturn::SUCCESS; new_req->maneuver_plan.maneuvers.push_back(i); } - new_req->header = req->header; - new_req->vehicle_state = req->vehicle_state; - new_req->initial_trajectory_plan = req->initial_trajectory_plan; - object_->call(new_req,resp); + new_req->header = req->header; + new_req->vehicle_state = req->vehicle_state; + new_req->initial_trajectory_plan = req->initial_trajectory_plan; + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr ilc_resp = std::make_shared(); + object_->call(new_req,ilc_resp); - if(!resp->trajectory_plan.trajectory_points.empty())//Since we're using an interface for this process, the call() functionality will come from somewhere else + if(ilc_resp->trajectory_plan.trajectory_points.empty())//Since we're using an interface for this process, the call() functionality will come from somewhere else { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Call Successful"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Failed to call service"); + return ; } else { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Failed to call service"); - return ; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Call Successful"); + resp->trajectory_plan = ilc_resp->trajectory_plan; } diff --git a/intersection_transit_maneuvering/src/itm_service.cpp b/intersection_transit_maneuvering/src/itm_service.cpp index e882e9bbb1..0180c2bb39 100644 --- a/intersection_transit_maneuvering/src/itm_service.cpp +++ b/intersection_transit_maneuvering/src/itm_service.cpp @@ -32,7 +32,7 @@ namespace intersection_transit_maneuvering Servicer::Servicer(){}; - void Servicer::call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + void Servicer::call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp) { std::shared_future resp_future = client->async_send_request(req); @@ -40,10 +40,12 @@ namespace intersection_transit_maneuvering if (future_status == std::future_status::ready) { resp = resp_future.get(); + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"), "Responsible tactical plugin was called and got trajectory size: " << resp->trajectory_plan.trajectory_points.size()); } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"), "failed to call plugin from" << "intersection_transit_maneuvering"); + RCLCPP_DEBUG(rclcpp::get_logger("intersection_transit_maneuvering"), "Failed to call the responsible tactical plugin from intersection_transit_maneuvering"); } return; diff --git a/intersection_transit_maneuvering/test/call_test.cpp b/intersection_transit_maneuvering/test/call_test.cpp index 7e9cbc0532..f73ed4f157 100644 --- a/intersection_transit_maneuvering/test/call_test.cpp +++ b/intersection_transit_maneuvering/test/call_test.cpp @@ -21,11 +21,11 @@ namespace call_test { - void CallTest::call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + void CallTest::call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp) { request = req; + resp->trajectory_plan = req->initial_trajectory_plan; response = resp; - return; } diff --git a/intersection_transit_maneuvering/test/call_test.hpp b/intersection_transit_maneuvering/test/call_test.hpp index a9fd99f966..06a3974e70 100644 --- a/intersection_transit_maneuvering/test/call_test.hpp +++ b/intersection_transit_maneuvering/test/call_test.hpp @@ -45,7 +45,7 @@ class CallTest: public CallInterface * @param resp Incoming PlanTrajectory service response * @return true if method successfully completes, otherwise false */ - void call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); + void call(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp); /** * @brief Get the Request object @@ -65,8 +65,8 @@ class CallTest: public CallInterface private: - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr request = std::make_shared ();; - carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr response = std::make_shared ();; + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr request = std::make_shared (); + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr response = std::make_shared (); diff --git a/intersection_transit_maneuvering/test/test_intersection_transit_maneuvering.cpp b/intersection_transit_maneuvering/test/test_intersection_transit_maneuvering.cpp index 50539f3d5f..bd045c86b4 100644 --- a/intersection_transit_maneuvering/test/test_intersection_transit_maneuvering.cpp +++ b/intersection_transit_maneuvering/test/test_intersection_transit_maneuvering.cpp @@ -209,8 +209,8 @@ namespace intersection_transit_maneuvering ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN, converted[i].lane_following_maneuver.parameters.presence_vector); ASSERT_EQ(0.0, converted[i].lane_following_maneuver.start_dist); ASSERT_EQ(5.0, converted[i].lane_following_maneuver.end_dist); - ASSERT_EQ(rclcpp::Time(1e9 * 0.0), converted[i].lane_following_maneuver.start_time); - ASSERT_EQ(rclcpp::Time(1e9 * 1.7701), converted[i].lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(1e9 * 0.0, rclcpp::Time(converted[i].lane_following_maneuver.start_time).get_clock_type()), converted[i].lane_following_maneuver.start_time); + ASSERT_EQ(rclcpp::Time(1e9 * 1.7701, rclcpp::Time(converted[i].lane_following_maneuver.end_time).get_clock_type()), converted[i].lane_following_maneuver.end_time); ASSERT_EQ(25.0, converted[i].lane_following_maneuver.start_speed); ASSERT_EQ(1.0, converted[i].lane_following_maneuver.end_speed); } diff --git a/lci_strategic_plugin/config/parameters.yaml b/lci_strategic_plugin/config/parameters.yaml index 5ccd7361ff..e79b68f28a 100755 --- a/lci_strategic_plugin/config/parameters.yaml +++ b/lci_strategic_plugin/config/parameters.yaml @@ -30,7 +30,7 @@ desired_distance_to_stop_buffer : 15.0 min_maneuver_planning_period : 15.1 # Bool: If enable_carma_streets_connection is true when we want to allow carma streets functionality (UC3) and if its false that means we don't want to allow carma streets behaviour and will only use UC2 behaviour. -enable_carma_streets_connection : true +enable_carma_streets_connection : false # Double: Mobility operation rate mobility_rate : 10.0 diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index f98c2183ae..79d9536ae2 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -1371,6 +1371,11 @@ void LCIStrategicPlugin::plan_maneuvers_callback( { current_light_state_optional = nearest_traffic_signal->predictState(lanelet::time::timeFromSec(current_state.stamp.seconds())); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Real-time current signal: " << current_light_state_optional.get().second << ", for Id: " << nearest_traffic_signal->id()); + if (current_light_state_optional.get().second == lanelet::CarmaTrafficSignalState::UNAVAILABLE || !current_light_state_optional) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "Signal state not available returning..." ); + return; + } } switch (transition_table_.getState()) { diff --git a/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp b/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp index 059e4ace8e..369dfdd67e 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp @@ -55,6 +55,24 @@ rclcpp::Time LCIStrategicPlugin::get_nearest_green_entry_time(const rclcpp::Time boost::posix_time::ptime t = lanelet::time::timeFromSec(current_time.seconds()); // time variable boost::posix_time::ptime eet = lanelet::time::timeFromSec(earliest_entry_time.seconds()); // earliest entry time + // check if the signal even has a green signal + bool has_green_signal = false; + for (auto pair : signal->recorded_time_stamps) + { + if (pair.second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED && + t <= pair.first ) + { + has_green_signal = true; + break; + } + } + + if (!has_green_signal) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "No Green signal found returning TBD at: " << std::to_string(rclcpp::Time((lanelet::time::toSec(signal->recorded_time_stamps.back().first) + EPSILON) * 1e9).seconds())); + return rclcpp::Time((lanelet::time::toSec(signal->recorded_time_stamps.back().first) + EPSILON) * 1e9); //return TBD red if no green is found... + } + auto curr_pair = signal->predictState(t); if (!curr_pair) throw std::invalid_argument("Traffic signal with id:" + std::to_string(signal->id()) + ", does not have any recorded time stamps!"); diff --git a/motion_computation/CMakeLists.txt b/motion_computation/CMakeLists.txt index 8d286c9912..fcaf9386aa 100644 --- a/motion_computation/CMakeLists.txt +++ b/motion_computation/CMakeLists.txt @@ -1,90 +1,108 @@ - -# Copyright (C) 2020-2022 LEIDOS. +# Copyright 2020-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 +# 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 +# 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. +# 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. cmake_minimum_required(VERSION 3.5) project(motion_computation) -# Declare carma package and check ROS version +# CARMA builds packages in an environment with both ROS 1 and ROS 2 installed. +# This check gracefully skips the current package if the sourced ROS environment +# is not the specified version. This call must come before any other ROS +# dependencies becasue ROS 1 does not have some of the required packages. find_package(carma_cmake_common REQUIRED) carma_check_ros_version(2) -carma_package() -## Find dependencies using ament auto + +# Added outside of `dependencies.cmake` because ament sets some variables +# (e.g., BUILD_TESTING) that affect the configuration options for the rest of +# the package. Putting the command call here allows us to put all project +# options together in separate CMake module then query those options in +# `dependencies.cmake`. find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() -# Name build targets -set(worker_lib motion_computation_worker_lib) -set(node_exec motion_computation_node_exec) -set(node_lib motion_computation_node_lib) +include(options.cmake) +include(dependencies.cmake) -# Includes -include_directories( - include -) +# The generated compilation database is helpful with code completion in IDEs +set(CMAKE_EXPORT_COMPILE_COMMANDS ${motion_computation_EXPORT_COMPILE_COMMANDS}) -# boost::posix_time definition for using nanoseconds -add_definitions(-DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG) +# This prevents `colcon` from tyring to build the CMake project's binary +# directory. This is useful in case we want to build the project outside of +# `colcon` but still keep it in a ROS repository. +file(TOUCH ${PROJECT_BINARY_DIR}/COLCON_IGNORE) -# Build +# Configures CARMA package default settings +carma_package() -ament_auto_add_library(${worker_lib} SHARED - src/motion_computation_worker.cpp - src/mobility_path_to_external_object.cpp - src/psm_to_external_object_convertor.cpp - src/bsm_to_external_object_convertor.cpp +# C17 CMake support added in CMake 3.21 +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +# ament_auto will automatically add include files from include/motion_computation +# and link necessary libraries +ament_auto_add_library(motion_computation SHARED + src/motion_computation_worker.cpp + src/mobility_path_to_external_object.cpp + src/psm_to_external_object_convertor.cpp + src/bsm_to_external_object_convertor.cpp + src/motion_computation_node.cpp ) -ament_auto_add_library(${node_lib} SHARED - src/motion_computation_node.cpp +ament_auto_add_executable(motion_computation_node_exec + src/main.cpp ) -ament_auto_add_executable(${node_exec} - src/main.cpp +rclcpp_components_register_nodes(motion_computation + "motion_computation::MotionComputationNode" ) -# Register component -rclcpp_components_register_nodes(${node_lib} "motion_computation::MotionComputationNode") - -# All locally created targets will need to be manually linked -# ament auto will handle linking of external dependencies -target_link_libraries(${node_lib} - ${worker_lib} -) +if(motion_computation_BUILD_TESTS) -target_link_libraries(${node_exec} - ${node_lib} -) + ament_auto_add_gtest(test_motion_computation + test/MotionComputationTest.cpp + test/TestMain.cpp + ) -# Testing -if(BUILD_TESTING) + # boost::posix_time definition for using nanoseconds + target_compile_definitions(test_motion_computation + PRIVATE + -DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG + ) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + add_launch_test(test/motion_computation_external_object_launch_test.py) + add_launch_test(test/motion_computation_queueing_launch_test.py) - ament_add_gtest(test_motion_computation - test/MotionComputationTest.cpp - test/TestMain.cpp) +endif() - ament_target_dependencies(test_motion_computation ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) - target_link_libraries(test_motion_computation ${node_lib}) +include(GNUInstallDirs) -endif() +# ament_auto does not provide file pattern matching +install(DIRECTORY ${PROJECT_SOURCE_DIR}/test/ + DESTINATION ${CMAKE_INSTALL_DATADIR}/motion_computation/test + FILES_MATCHING + PATTERN *.py + PATTERN *.cpp + PATTERN __pycache__ EXCLUDE +) -# Install -ament_auto_package( - INSTALL_TO_SHARE config launch +install(DIRECTORY ${PROJECT_SOURCE_DIR}/test/data/ + DESTINATION ${CMAKE_INSTALL_DATADIR}/motion_computation/test/data + FILES_MATCHING + PATTERN *.yaml ) diff --git a/motion_computation/README.md b/motion_computation/README.md index 4a6776a748..430443bca4 100644 --- a/motion_computation/README.md +++ b/motion_computation/README.md @@ -1,3 +1,19 @@ -# motion_computation +# CARMA motion computation package -The motion_computation package contains a node that subscribes to ROS topics containing position, velocity, and other relevant information for external objects. Using this received data, this node predicts the future location of each of these external objects, and publishes those predictions for other nodes within the CARMA system to utilize. +The `motion_computation` package contains a node that subscribes to ROS topics +containing position, velocity, and other relevant information for external +objects. Using this received data, this node predicts the future location of +each of these external objects, and publishes those predictions for other nodes +within the CARMA system to utilize. + +## Package Nodes + +- Motion computation Node: [`motion_computation_node`][motion_computation_node_docs] + +[motion_computation_node_docs]: docs/motion_computation_node.md + +## Package Launch files + +- Motion computation Launch: [`motion_computation_launch.py`][motion_computation_launch_docs] + +[motion_computation_launch_docs]: docs/motion_computation_launch.md diff --git a/motion_computation/cmake/ament_auto_add_gtest.cmake b/motion_computation/cmake/ament_auto_add_gtest.cmake new file mode 100644 index 0000000000..f723f9c9bb --- /dev/null +++ b/motion_computation/cmake/ament_auto_add_gtest.cmake @@ -0,0 +1,112 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# 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. + +# +# Add a gtest with all found test dependencies. +# +# Call add_executable(target ARGN), link it against the gtest libraries +# and all found test dependencies, and then register the executable as a test. +# +# If gtest is not available the specified target is not being created and +# therefore the target existence should be checked before being used. +# +# :param target: the target name which will also be used as the test name +# :type target: string +# :param ARGN: the list of source files +# :type ARGN: list of strings +# :param RUNNER: the path to the test runner script (default: +# see ament_add_test). +# :type RUNNER: string +# :param TIMEOUT: the test timeout in seconds, +# default defined by ``ament_add_test()`` +# :type TIMEOUT: integer +# :param WORKING_DIRECTORY: the working directory for invoking the +# executable in, default defined by ``ament_add_test()`` +# :type WORKING_DIRECTORY: string +# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest +# main libraries +# :type SKIP_LINKING_MAIN_LIBRARIES: option +# :param SKIP_TEST: if set mark the test as being skipped +# :type SKIP_TEST: option +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# +# @public +# +macro(ament_auto_add_gtest target) + cmake_parse_arguments(_ARG + "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + ${ARGN}) + if(NOT _ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR + "ament_auto_add_gtest() must be invoked with at least one source file") + endif() + + # add executable + set(_arg_executable ${_ARG_UNPARSED_ARGUMENTS}) + if(_ARG_SKIP_LINKING_MAIN_LIBRARIES) + list(APPEND _arg_executable "SKIP_LINKING_MAIN_LIBRARIES") + endif() + ament_add_gtest_executable("${target}" ${_arg_executable}) + + # add include directory of this package if it exists + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include") + target_include_directories("${target}" PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include") + endif() + + # link against other libraries of this package + if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "") + target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES}) + endif() + + # add exported information from found dependencies + ament_target_dependencies(${target} + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} + ) + + # add test + set(_arg_test "") + if(_ARG_RUNNER) + list(APPEND _arg_test "RUNNER" "${_ARG_RUNNER}") + endif() + if(_ARG_TIMEOUT) + list(APPEND _arg_test "TIMEOUT" "${_ARG_TIMEOUT}") + endif() + if(_ARG_WORKING_DIRECTORY) + list(APPEND _arg_test "WORKING_DIRECTORY" "${_ARG_WORKING_DIRECTORY}") + endif() + if(_ARG_SKIP_TEST) + list(APPEND _arg_test "SKIP_TEST") + endif() + if(_ARG_ENV) + list(APPEND _arg_test "ENV" ${_ARG_ENV}) + endif() + if(_ARG_APPEND_ENV) + list(APPEND _arg_test "APPEND_ENV" ${_ARG_APPEND_ENV}) + endif() + if(_ARG_APPEND_LIBRARY_DIRS) + list(APPEND _arg_test "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS}) + endif() + ament_add_gtest_test("${target}" ${_arg_test}) +endmacro() diff --git a/motion_computation/cmake/ament_auto_find_test_dependencies.cmake b/motion_computation/cmake/ament_auto_find_test_dependencies.cmake new file mode 100644 index 0000000000..8320b26e14 --- /dev/null +++ b/motion_computation/cmake/ament_auto_find_test_dependencies.cmake @@ -0,0 +1,41 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# 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. + +# +# Invoke find_package() for all test dependencies. +# +# All found package names are appended to the +# ``${PROJECT_NAME}_FOUND_TEST_DEPENDS`` variables. +# +# @public +# +macro(ament_auto_find_test_dependencies) + set(_ARGN "${ARGN}") + if(_ARGN) + message(FATAL_ERROR "ament_auto_find_test_dependencies() called with " + "unused arguments: ${_ARGN}") + endif() + + if(NOT _AMENT_PACKAGE_NAME) + ament_package_xml() + endif() + + # try to find_package() all test dependencies + foreach(_dep ${${PROJECT_NAME}_TEST_DEPENDS}) + find_package(${_dep} QUIET) + if(${_dep}_FOUND) + list(APPEND ${PROJECT_NAME}_FOUND_TEST_DEPENDS ${_dep}) + endif() + endforeach() +endmacro() diff --git a/motion_computation/dependencies.cmake b/motion_computation/dependencies.cmake new file mode 100644 index 0000000000..f4a308ae7c --- /dev/null +++ b/motion_computation/dependencies.cmake @@ -0,0 +1,33 @@ +# Copyright 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. + +# This will pull dependencies from ... tags in the +# package.xml file. It saves us from having to manually call find_package(...) +# for each dependency. +ament_auto_find_build_dependencies() + +if(motion_computation_BUILD_TESTS) + # These CMake commands were added to ament_cmake_auto in ROS 2 Humble. Until + # CARMA supports ROS 2 Humble, we will use package-local copies. + include(cmake/ament_auto_find_test_dependencies.cmake) + include(cmake/ament_auto_add_gtest.cmake) + + ament_auto_find_test_dependencies() + + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify # Using clang-format instead + ) + + ament_lint_auto_find_test_dependencies() +endif() diff --git a/motion_computation/docs/motion_computation_launch.md b/motion_computation/docs/motion_computation_launch.md new file mode 100644 index 0000000000..9f2ede6d98 --- /dev/null +++ b/motion_computation/docs/motion_computation_launch.md @@ -0,0 +1,7 @@ +# Motion computation Launch + +## Launch arguments + +| Name | Default value | Description | +| ----------- | ------------- | ---------------------------------------------------------------------- | +| `log_level` | `WARN` | Severity level at or above which the associated message with be logged | diff --git a/motion_computation/docs/motion_computation_node.md b/motion_computation/docs/motion_computation_node.md new file mode 100644 index 0000000000..fa0d2d1ca9 --- /dev/null +++ b/motion_computation/docs/motion_computation_node.md @@ -0,0 +1,74 @@ +# Motion computation Node + +This Node is responsible for predicting environment objects' motion and sharing those predictions with the rest of +CARMA Platform. It serves three main functions: + +- Convert incoming BSM, PSM, and mobility path data into `ExternalObject`s +- Predict incoming objects' motion using either the constant velocity (CV) or constant turn-rate and velocity (CTRV) + model +- Queue and synchronize incoming object information so that it gets published all at once at a specific frequency + +The following table lists the motion model used for each `ExternalObject` semantic category + +| Object type | Motion model | +| --------------- | ------------ | +| `UNKNOWN` | CTRV | +| `MOTORCYCLE` | CTRV | +| `SMALL_VEHICLE` | CTRV | +| `LARGE_VEHICLE` | CTRV | +| `PEDESTRIAN` | CV | +| default | CV | + +> [!IMPORTANT]\ +> This Node requires a valid georeference to do the motion predictions. Until it receives one on the `georeference` +> topic, it will not publish any. + +> [!IMPORTANT]\ +> This Node will only publish motion predictions when it receives external object information on the `external_objects` +> topic. Messages on the `incoming_*` topics will be queued until then. + +## Subscriptions + +| Topic | Message Type | Description | +| ------------------------ | --------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------- | +| `external_objects` | [`carma_perception_msgs::msg::ExternalObjectList`][external_object_list_msg_link] | Locally-detected objects | +| `incoming_mobility_path` | [`carma_v2x_msgs::msg::MobilityPath`][mobility_path_msg_link] | Self-reported planned motion paths from nearby vehicles | +| `incoming_bsm` | [`carma_v2x_msgs::msg::BSM`][bsm_msg_link] | Incoming basic safety messages | +| `incoming_psm` | [`carma_v2x_msgs::msg::PSM`][psm_msg_link] | Incoming personal safety messages | +| `georeference` | [`std_msgs::msg::String`][string_msg_link] | Georeference point for projecting WGS-84 coordinates to a plane. Assumed and required to be a valid PROJ string. | + +## Publishers + +| Topic | Message Type | Frequency | Description | +| ----------------------------- | --------------------------------------------------------------------------------- | ------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `external_object_predictions` | [`carma_perception_msgs::msg::ExternalObjectList`][external_object_list_msg_link] | Subscription-driven | This Node will publish predictions only when it receives a message from the `external_objects` topic. Incoming mobility paths, BSMs, and PSMs will be queued until then. | + +## Parameters + +| Topic | Data Type | Default Value | Required | Read Only | Description | +| --------------------------------- | --------- | ------------- | -------- | --------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `prediction_time_step` | `double` | `0.1` | Yes | No | Motion prediction time step (in seconds) | +| `mobility_path_time_step` | `double` | `0.1` | Yes | No | Time between received mobility path predicted states (in seconds) | +| `prediction_period` | `double` | `2.0` | Yes | No | Prediction horizon (in seconds) | +| `cv_x_accel_noise` | `double` | `9.0` | Yes | No | CV Model X-Axis Acceleration Noise | +| `cv_y_accel_noise` | `double` | `9.0` | Yes | No | CV Model Y-Axis Acceleration Noise | +| `prediction_process_noise_max` | `double` | `1000.0` | Yes | No | Maximum expected process noise; used for mapping noise to confidence in _[0,1]_ range | +| `prediction_confidence_drop_rate` | `double` | `0.95` | Yes | No | Percentage of initial confidence to propagate to next time step | +| `enable_bsm_processing` | `bool` | `false` | Yes | No | If `true` then BSM messages will be converted to `ExternalObjects`. If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) | +| `enable_psm_processing` | `bool` | `false` | Yes | No | If `true` then PSM messages will be converted to `ExternalObjects`. If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) | +| `enable_mobility_path_processing` | `bool` | `true` | Yes | No | If `true` then MobilityPath messages will be converted to `ExternalObjects`. If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) | +| `enable_sensor_processing` | `bool` | `false` | Yes | No | If `true` then `ExternalObjects` generated from sensor data will be processed. If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) | + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. + +[external_object_list_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_perception_msgs/msg/ExternalObjectList.msg +[mobility_path_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_v2x_msgs/msg/MobilityPath.msg +[bsm_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_v2x_msgs/msg/BSM.msg +[psm_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_v2x_msgs/msg/PSM.msg +[string_msg_link]: http://docs.ros.org/en/api/std_msgs/html/msg/String.html diff --git a/motion_computation/include/motion_computation/impl/mobility_path_to_external_object_helpers.hpp b/motion_computation/include/motion_computation/impl/mobility_path_to_external_object_helpers.hpp index f694fb000f..aea33ad123 100644 --- a/motion_computation/include/motion_computation/impl/mobility_path_to_external_object_helpers.hpp +++ b/motion_computation/include/motion_computation/impl/mobility_path_to_external_object_helpers.hpp @@ -1,17 +1,37 @@ -#pragma once +// Copyright 2020-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. +#ifndef MOTION_COMPUTATION__IMPL__MOBILITY_PATH_TO_EXTERNAL_OBJECT_HELPERS_HPP_ +#define MOTION_COMPUTATION__IMPL__MOBILITY_PATH_TO_EXTERNAL_OBJECT_HELPERS_HPP_ + +#include #include #include +#include #include #include #include #include -#include +#include -namespace motion_computation { +namespace motion_computation +{ -namespace conversion { -namespace impl { +namespace conversion +{ +namespace impl +{ /** * \brief Composes a PredictedState message form a provided current point and previous point. It * calculates the speed from these points using mobility_path_time_step @@ -24,24 +44,22 @@ namespace impl { * including linear velocity, last_time, orientation filled in and the second element is the yaw in radians used to * compute the orientation */ -std::pair composePredictedState(const tf2::Vector3 &curr_pt, - const tf2::Vector3 &prev_pt, - const rclcpp::Time &prev_time_stamp, - const rclcpp::Time &curr_time_stamp, - double prev_yaw); +std::pair composePredictedState( + const tf2::Vector3 & curr_pt, const tf2::Vector3 & prev_pt, const rclcpp::Time & prev_time_stamp, + const rclcpp::Time & curr_time_stamp, double prev_yaw); /** * \brief Helper function to fill in the angular velocity of the external object * \param ExternalObject to fill in its angular velocities */ -void calculateAngVelocityOfPredictedStates(carma_perception_msgs::msg::ExternalObject &object); +void calculateAngVelocityOfPredictedStates(carma_perception_msgs::msg::ExternalObject & object); /** * \brief Gets the yaw angle in degrees described by the provided quaternion - * + * * \param quaternion The quaternion to extract yaw from * \return The yaw in degrees - */ -double getYawFromQuaternionMsg(const geometry_msgs::msg::Quaternion &quaternion); + */ +double getYawFromQuaternionMsg(const geometry_msgs::msg::Quaternion & quaternion); /** * \brief Transforms ecef point to map frame using internally saved map transform @@ -49,8 +67,11 @@ double getYawFromQuaternionMsg(const geometry_msgs::msg::Quaternion &quaternion) * \param map_projector Projector used for frame conversion * \return point in map */ -tf2::Vector3 transform_to_map_frame(const tf2::Vector3 &ecef_point, const lanelet::projection::LocalFrameProjector &map_projector); +tf2::Vector3 transform_to_map_frame( + const tf2::Vector3 & ecef_point, const lanelet::projection::LocalFrameProjector & map_projector); } // namespace impl } // namespace conversion } // namespace motion_computation + +#endif // MOTION_COMPUTATION__IMPL__MOBILITY_PATH_TO_EXTERNAL_OBJECT_HELPERS_HPP_ diff --git a/motion_computation/include/motion_computation/impl/psm_to_external_object_helpers.hpp b/motion_computation/include/motion_computation/impl/psm_to_external_object_helpers.hpp index 4c5b2a70ac..3e2f46908c 100644 --- a/motion_computation/include/motion_computation/impl/psm_to_external_object_helpers.hpp +++ b/motion_computation/include/motion_computation/impl/psm_to_external_object_helpers.hpp @@ -1,61 +1,65 @@ -#pragma once - -/* - * 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 - * 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. - */ +// Copyright 2020-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. +#ifndef MOTION_COMPUTATION__IMPL__PSM_TO_EXTERNAL_OBJECT_HELPERS_HPP_ +#define MOTION_COMPUTATION__IMPL__PSM_TO_EXTERNAL_OBJECT_HELPERS_HPP_ + +#include +#include +#include #include #include #include #include -#include -#include -#include #include +#include +#include +namespace motion_computation +{ - - -namespace motion_computation { - -namespace conversion { - -namespace impl // Namespace for functionality not meant to be part of public API but valuable to unit test in - // isolation +namespace conversion { -std::vector sample_2d_path_from_radius(const geometry_msgs::msg::Pose &pose, double velocity, - double radius_of_curvature, double period, - double step_size); -std::vector sample_2d_linear_motion(const geometry_msgs::msg::Pose &pose, double velocity, - double period, double step_size); +// Namespace for functionality not meant to be part of public API but valuable +// to unit test in isolation +namespace impl +{ +std::vector sample_2d_path_from_radius( + const geometry_msgs::msg::Pose & pose, double velocity, double radius_of_curvature, double period, + double step_size); +std::vector sample_2d_linear_motion( + const geometry_msgs::msg::Pose & pose, double velocity, double period, double step_size); -geometry_msgs::msg::PoseWithCovariance pose_from_gnss(const lanelet::projection::LocalFrameProjector &projector, - const tf2::Quaternion &ned_in_map_rotation, const lanelet::GPSPoint &gps_point, - const double &heading, const double lat_variance, - const double lon_variance, const double heading_variance); +geometry_msgs::msg::PoseWithCovariance pose_from_gnss( + const lanelet::projection::LocalFrameProjector & projector, + const tf2::Quaternion & ned_in_map_rotation, const lanelet::GPSPoint & gps_point, + const double & heading, const double lat_variance, const double lon_variance, + const double heading_variance); std::vector predicted_poses_to_predicted_state( - const std::vector &poses, double constant_velocity, const rclcpp::Time &start_time, - const rclcpp::Duration &step_size, const std::string &frame, double initial_pose_confidence, - double initial_vel_confidence); + const std::vector & poses, double constant_velocity, + const rclcpp::Time & start_time, const rclcpp::Duration & step_size, const std::string & frame, + double initial_pose_confidence, double initial_vel_confidence); -rclcpp::Time get_psm_timestamp(const carma_v2x_msgs::msg::PSM &in_msg, rclcpp::Clock::SharedPtr clock); +rclcpp::Time get_psm_timestamp( + const carma_v2x_msgs::msg::PSM & in_msg, rclcpp::Clock::SharedPtr clock); } // namespace impl } // namespace conversion -} // namespace motion_computation \ No newline at end of file +} // namespace motion_computation + +#endif // MOTION_COMPUTATION__IMPL__PSM_TO_EXTERNAL_OBJECT_HELPERS_HPP_ diff --git a/motion_computation/include/motion_computation/message_conversions.hpp b/motion_computation/include/motion_computation/message_conversions.hpp index 40f9f4405c..8e1f449173 100644 --- a/motion_computation/include/motion_computation/message_conversions.hpp +++ b/motion_computation/include/motion_computation/message_conversions.hpp @@ -1,25 +1,53 @@ +// Copyright 2020-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. + +#ifndef MOTION_COMPUTATION__MESSAGE_CONVERSIONS_HPP_ +#define MOTION_COMPUTATION__MESSAGE_CONVERSIONS_HPP_ + +#include +#include #include -#include #include #include -#include -#include +#include #include +#include -namespace motion_computation { +namespace motion_computation +{ -namespace conversion { +namespace conversion +{ -void convert(const carma_v2x_msgs::msg::PSM& in_msg, carma_perception_msgs::msg::ExternalObject& out_msg, - const std::string& map_frame_id, double pred_period, double pred_step_size, - const lanelet::projection::LocalFrameProjector& map_projector, const tf2::Quaternion& ned_in_map_rotation, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock); +void convert( + const carma_v2x_msgs::msg::PSM & in_msg, carma_perception_msgs::msg::ExternalObject & out_msg, + const std::string & map_frame_id, double pred_period, double pred_step_size, + const lanelet::projection::LocalFrameProjector & map_projector, + const tf2::Quaternion & ned_in_map_rotation, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock); -void convert(const carma_v2x_msgs::msg::BSM& in_msg, carma_perception_msgs::msg::ExternalObject& out_msg, - const std::string& map_frame_id, double pred_period, double pred_step_size, - const lanelet::projection::LocalFrameProjector& map_projector, tf2::Quaternion ned_in_map_rotation); +void convert( + const carma_v2x_msgs::msg::BSM & in_msg, carma_perception_msgs::msg::ExternalObject & out_msg, + const std::string & map_frame_id, double pred_period, double pred_step_size, + const lanelet::projection::LocalFrameProjector & map_projector, + tf2::Quaternion ned_in_map_rotation); -void convert(const carma_v2x_msgs::msg::MobilityPath &in_msg, carma_perception_msgs::msg::ExternalObject &out_msg, - const lanelet::projection::LocalFrameProjector &map_projector); +void convert( + const carma_v2x_msgs::msg::MobilityPath & in_msg, + carma_perception_msgs::msg::ExternalObject & out_msg, + const lanelet::projection::LocalFrameProjector & map_projector); } // namespace conversion -} // namespace motion_computation \ No newline at end of file +} // namespace motion_computation + +#endif // MOTION_COMPUTATION__MESSAGE_CONVERSIONS_HPP_ diff --git a/motion_computation/include/motion_computation/motion_computation_config.hpp b/motion_computation/include/motion_computation/motion_computation_config.hpp index abb0c9f1f5..be1f1f6544 100644 --- a/motion_computation/include/motion_computation/motion_computation_config.hpp +++ b/motion_computation/include/motion_computation/motion_computation_config.hpp @@ -1,56 +1,65 @@ -#pragma once +// Copyright 2020-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. -/* - * 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 - * 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. - */ +#ifndef MOTION_COMPUTATION__MOTION_COMPUTATION_CONFIG_HPP_ +#define MOTION_COMPUTATION__MOTION_COMPUTATION_CONFIG_HPP_ #include #include -namespace motion_computation { +namespace motion_computation +{ /** * \brief Stuct containing the algorithm configuration values for motion_computation */ -struct Config { - double prediction_time_step = 0.1; // Time between predicted states (in seconds) - double mobility_path_time_step = 0.1; // Time between received mobility path predicted states (in seconds) - double prediction_period = 2.0; // Period of prediction (in seconds) - double cv_x_accel_noise = 9.0; // CV Model X-Axis Acceleration Noise - double cv_y_accel_noise = 9.0; // CV Model Y-Axis Acceleration Noise - double prediction_process_noise_max = 1000.0; // Maximum expected process noise; used for mapping noise to confidence in [0,1] range - double prediction_confidence_drop_rate = 0.95; // Percentage of initial confidence to propagate to next time step +struct Config +{ + double prediction_time_step = 0.1; // Time between predicted states (in seconds) + double mobility_path_time_step = + 0.1; // Time between received mobility path predicted states (in seconds) + double prediction_period = 2.0; // Period of prediction (in seconds) + double cv_x_accel_noise = 9.0; // CV Model X-Axis Acceleration Noise + double cv_y_accel_noise = 9.0; // CV Model Y-Axis Acceleration Noise + double prediction_process_noise_max = + 1000.0; // Maximum expected process noise; used for mapping noise to confidence in [0,1] range + double prediction_confidence_drop_rate = + 0.95; // Percentage of initial confidence to propagate to next time step // If true then BSM messages will be converted to ExternalObjects. - // If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) + // If other object sources are enabled, they will be synchronized but no fusion + // will occur (objects may be duplicated) bool enable_bsm_processing = false; // If true then PSM messages will be converted to ExternalObjects. - // If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) + // If other object sources are enabled, they will be synchronized but no fusion + // will occur (objects may be duplicated) bool enable_psm_processing = false; // If true then MobilityPath messages will be converted to ExternalObjects. - // If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) + // If other object sources are enabled, they will be synchronized but no fusion + // will occur (objects may be duplicated) bool enable_mobility_path_processing = true; // If true then ExternalObjects generated from sensor data will be processed. - // If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) + // If other object sources are enabled, they will be synchronized but no fusion + // will occur (objects may be duplicated) bool enable_sensor_processing = false; // Stream operator for this config - friend std::ostream &operator<<(std::ostream &output, const Config &c) { + friend std::ostream & operator<<(std::ostream & output, const Config & c) + { output << "motion_computation::Config { " << std::endl << "prediction_time_step: " << c.prediction_time_step << std::endl << "mobility_path_time_step: " << c.mobility_path_time_step << std::endl @@ -68,4 +77,6 @@ struct Config { } }; -} // namespace motion_computation \ No newline at end of file +} // namespace motion_computation + +#endif // MOTION_COMPUTATION__MOTION_COMPUTATION_CONFIG_HPP_ diff --git a/motion_computation/include/motion_computation/motion_computation_node.hpp b/motion_computation/include/motion_computation/motion_computation_node.hpp index ed68310f1f..5732a2ae7b 100644 --- a/motion_computation/include/motion_computation/motion_computation_node.hpp +++ b/motion_computation/include/motion_computation/motion_computation_node.hpp @@ -1,42 +1,44 @@ -/* - * Copyright (C) 2019-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 - * 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. - */ +// Copyright 2019-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. -#ifndef MOTION_COMPUTATION_H -#define MOTION_COMPUTATION_H +#ifndef MOTION_COMPUTATION__MOTION_COMPUTATION_NODE_HPP_ +#define MOTION_COMPUTATION__MOTION_COMPUTATION_NODE_HPP_ #include #include #include +#include #include -#include #include #include -#include +#include +#include + #include "motion_computation/motion_computation_config.hpp" #include "motion_computation/motion_computation_worker.hpp" -namespace motion_computation { +namespace motion_computation +{ /** * \class MotionComputationNode * \brief The class responsible for publishing external object predictions */ -class MotionComputationNode : public carma_ros2_utils::CarmaLifecycleNode { - private: +class MotionComputationNode : public carma_ros2_utils::CarmaLifecycleNode +{ +private: // Subscribers carma_ros2_utils::SubPtr motion_comp_sub_; carma_ros2_utils::SubPtr mobility_path_sub_; @@ -53,7 +55,7 @@ class MotionComputationNode : public carma_ros2_utils::CarmaLifecycleNode { // Node configuration Config config_; - public: +public: /** * \brief MotionComputationNode constructor */ @@ -62,13 +64,14 @@ class MotionComputationNode : public carma_ros2_utils::CarmaLifecycleNode { /** * \brief Function callback for dynamic parameter updates */ - rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); + rcl_interfaces::msg::SetParametersResult parameter_update_callback( + const std::vector & parameters); /** * \brief Function to publish ExternalObjectList * \param obj_pred_msg ExternalObjectList message to be published */ - void publishObject(const carma_perception_msgs::msg::ExternalObjectList &obj_pred_msg) const; + void publishObject(const carma_perception_msgs::msg::ExternalObjectList & obj_pred_msg) const; //// // Overrides @@ -78,4 +81,4 @@ class MotionComputationNode : public carma_ros2_utils::CarmaLifecycleNode { } // namespace motion_computation -#endif /* MOTION_COMPUTATION_H */ +#endif // MOTION_COMPUTATION__MOTION_COMPUTATION_NODE_HPP_ diff --git a/motion_computation/include/motion_computation/motion_computation_worker.hpp b/motion_computation/include/motion_computation/motion_computation_worker.hpp index a38098efcd..c04bca01e3 100644 --- a/motion_computation/include/motion_computation/motion_computation_worker.hpp +++ b/motion_computation/include/motion_computation/motion_computation_worker.hpp @@ -1,22 +1,19 @@ -/* - * Copyright (C) 2019-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 - * 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. - */ - -#ifndef MOTION_COMPUTATION_WORKER_H -#define MOTION_COMPUTATION_WORKER_H - +// Copyright 2019-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. + +#ifndef MOTION_COMPUTATION__MOTION_COMPUTATION_WORKER_HPP_ +#define MOTION_COMPUTATION__MOTION_COMPUTATION_WORKER_HPP_ #include #include @@ -27,30 +24,38 @@ #include #include #include -#include #include #include #include #include + +#include +#include +#include #include +#include -namespace motion_computation { +namespace motion_computation +{ /** * \class MotionComputationWorker * \brief The class containing the primary business logic for the Motion Computation Package */ -class MotionComputationWorker { - public: - using PublishObjectCallback = std::function; +class MotionComputationWorker +{ +public: + using PublishObjectCallback = + std::function; using LookUpTransform = std::function; /*! * \brief Constructor for MotionComputationWorker */ - MotionComputationWorker(const PublishObjectCallback& obj_pub, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock); + MotionComputationWorker( + const PublishObjectCallback & obj_pub, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock); /** * \brief Function to populate duplicated detected objects along with their velocity, yaw, * yaw_rate and static/dynamic class to the provided ExternalObjectList message. @@ -65,8 +70,9 @@ class MotionComputationWorker { void setYAccelerationNoise(double noise); void setProcessNoiseMax(double noise_max); void setConfidenceDropRate(double drop_rate); - void setDetectionInputFlags(bool enable_sensor_processing, bool enable_bsm_processing, bool enable_psm_processing, - bool enable_mobility_path_processing); + void setDetectionInputFlags( + bool enable_sensor_processing, bool enable_bsm_processing, bool enable_psm_processing, + bool enable_mobility_path_processing); // callbacks void mobilityPathCallback(const carma_v2x_msgs::msg::MobilityPath::UniquePtr msg); @@ -88,7 +94,7 @@ class MotionComputationWorker { * \return ExternalObject object */ carma_perception_msgs::msg::ExternalObject mobilityPathToExternalObject( - const carma_v2x_msgs::msg::MobilityPath::UniquePtr& msg) const; + const carma_v2x_msgs::msg::MobilityPath::UniquePtr & msg) const; /** * \brief Appends external objects list behind base_objects. This does not do sensor fusion. @@ -100,8 +106,8 @@ class MotionComputationWorker { * \return append and synchronized list of external objects */ carma_perception_msgs::msg::ExternalObjectList synchronizeAndAppend( - const carma_perception_msgs::msg::ExternalObjectList& base_objects, - carma_perception_msgs::msg::ExternalObjectList new_objects) const; + const carma_perception_msgs::msg::ExternalObjectList & base_objects, + carma_perception_msgs::msg::ExternalObjectList new_objects) const; /*! * \brief It cuts ExternalObject's prediction points before the time_to_match. And uses the average @@ -113,10 +119,9 @@ class MotionComputationWorker { * \note It assumes time_to_match falls in prediction time's whole interval. */ carma_perception_msgs::msg::ExternalObject matchAndInterpolateTimeStamp( - carma_perception_msgs::msg::ExternalObject path, const rclcpp::Time& time_to_match) const; - - private: + carma_perception_msgs::msg::ExternalObject path, const rclcpp::Time & time_to_match) const; +private: // Local copy of external object publisher PublishObjectCallback obj_pub_; @@ -135,13 +140,12 @@ class MotionComputationWorker { bool enable_psm_processing_ = false; bool enable_mobility_path_processing_ = false; - - //Map frame + // Map frame std::string map_frame_id_ = "map"; // Logger interface rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; - //Clock interface - gets the ros simulated clock from Node + // Clock interface - gets the ros simulated clock from Node rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; // Queue for v2x msgs to synchronize them with sensor msgs @@ -149,7 +153,6 @@ class MotionComputationWorker { carma_perception_msgs::msg::ExternalObjectList bsm_list_; carma_perception_msgs::msg::ExternalObjectList psm_list_; - // Maps of external object id to index in synchronization queues std::unordered_map mobility_path_obj_id_map_; std::unordered_map bsm_obj_id_map_; @@ -161,11 +164,11 @@ class MotionComputationWorker { tf2::Quaternion ned_in_map_rotation_; // Unit Test Accessors - FRIEND_TEST(MotionComputationWorker, mobilityPathToExternalObject); - FRIEND_TEST(MotionComputationWorker, psmToExternalObject); + FRIEND_TEST(MotionComputationWorker, MobilityPathToExternalObject); + FRIEND_TEST(MotionComputationWorker, PsmToExternalObject); FRIEND_TEST(MotionComputationWorker, BSMtoExternalObject); }; } // namespace motion_computation -#endif /* MOTION_COMPUTATION_WORKER_H */ \ No newline at end of file +#endif // MOTION_COMPUTATION__MOTION_COMPUTATION_WORKER_HPP_ diff --git a/motion_computation/launch/motion_computation_launch.py b/motion_computation/launch/motion_computation_launch.py index c0b61e3c17..14c3b58e10 100644 --- a/motion_computation/launch/motion_computation_launch.py +++ b/motion_computation/launch/motion_computation_launch.py @@ -1,16 +1,16 @@ -# Copyright (C) 2022 LEIDOS. +# Copyright 2022-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 +# 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 +# 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. +# 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. from ament_index_python import get_package_share_directory from launch import LaunchDescription @@ -23,46 +23,43 @@ import os -''' +""" This file is can be used to launch the CARMA motion_computation_node. Though in carma-platform it may be launched directly from the base launch file. -''' +""" -def generate_launch_description(): +def generate_launch_description(): # Declare the log_level launch argument - log_level = LaunchConfiguration('log_level') + log_level = LaunchConfiguration("log_level") declare_log_level_arg = DeclareLaunchArgument( - name ='log_level', default_value='WARN') - + name="log_level", default_value="WARN" + ) + # Get parameter file path param_file_path = os.path.join( - get_package_share_directory('motion_computation'), 'config/parameters.yaml') + get_package_share_directory("motion_computation"), "config/parameters.yaml" + ) - # Launch node(s) in a carma container to allow logging to be configured container = ComposableNodeContainer( - package='carma_ros2_utils', - name='motion_computation_container', + package="carma_ros2_utils", + name="motion_computation_container", namespace=GetCurrentNamespace(), - executable='carma_component_container_mt', + executable="carma_component_container_mt", composable_node_descriptions=[ - # Launch the core node(s) ComposableNode( - package='motion_computation', - plugin='motion_computation::MotionComputationNode', - name='motion_computation_node', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : log_level } - ], - parameters=[ param_file_path ] + package="motion_computation", + plugin="motion_computation::MotionComputationNode", + name="motion_computation_node", + extra_arguments=[ + {"use_intra_process_comms": True}, + {"--log-level": log_level}, + ], + parameters=[param_file_path], ), - ] + ], ) - return LaunchDescription([ - declare_log_level_arg, - container - ]) + return LaunchDescription([declare_log_level_arg, container]) diff --git a/motion_computation/options.cmake b/motion_computation/options.cmake new file mode 100644 index 0000000000..a59730bd9c --- /dev/null +++ b/motion_computation/options.cmake @@ -0,0 +1,23 @@ +# Copyright 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. + +option(motion_computation_BUILD_TESTS + "Build package tests" + ${BUILD_TESTING} +) + +option(motion_computation_EXPORT_COMPILE_COMMANDS + "Export compile commands" + ON +) diff --git a/motion_computation/package.xml b/motion_computation/package.xml index f2c35911fb..48b72353b1 100644 --- a/motion_computation/package.xml +++ b/motion_computation/package.xml @@ -1,6 +1,6 @@ - @@ -21,10 +24,11 @@ carma Apache 2.0 - + ament_cmake + + ament_cmake_auto carma_cmake_common - ament_auto_cmake rclcpp carma_ros2_utils @@ -36,7 +40,11 @@ tf2_geometry_msgs ament_lint_auto + ament_lint_common + ament_cmake_clang_format ament_cmake_gtest + launch_testing_ament_cmake + rclpy launch launch_ros diff --git a/roadway_objects/src/main.cpp b/roadway_objects/src/main.cpp deleted file mode 100644 index 7c25082ee6..0000000000 --- a/roadway_objects/src/main.cpp +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2019-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 - * 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 -#include "roadway_objects/roadway_objects_node.hpp" - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(rclcpp::NodeOptions()); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - executor.spin(); - - rclcpp::shutdown(); - - return 0; -} diff --git a/roadway_objects/src/roadway_objects_component.cpp b/roadway_objects/src/roadway_objects_component.cpp new file mode 100644 index 0000000000..5fb32bab1f --- /dev/null +++ b/roadway_objects/src/roadway_objects_component.cpp @@ -0,0 +1,96 @@ +// Copyright 2019-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 "roadway_objects/roadway_objects_component.hpp" + +#include + +namespace roadway_objects +{ +namespace std_ph = std::placeholders; + +RoadwayObjectsNode::RoadwayObjectsNode(const rclcpp::NodeOptions & options) +: carma_ros2_utils::CarmaLifecycleNode(options) +{ +} + +auto RoadwayObjectsNode::handle_on_configure(const rclcpp_lifecycle::State &) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO_STREAM(get_logger(), "RoadwayObjectsNode trying to configure"); + + wm_listener_ = std::make_shared( + get_node_base_interface(), get_node_logging_interface(), get_node_topics_interface(), + get_node_parameters_interface()); + + roadway_obs_pub_ = + create_publisher("roadway_objects", 10); + + external_objects_sub_ = create_subscription( + "external_objects", 10, + [this](const carma_perception_msgs::msg::ExternalObjectList::SharedPtr msg_ptr) { + publish_obstacles(*msg_ptr); + }); + + return CallbackReturn::SUCCESS; +} + +auto RoadwayObjectsNode::publish_obstacles( + const carma_perception_msgs::msg::ExternalObjectList & msg) -> void +{ + carma_perception_msgs::msg::RoadwayObstacleList obstacle_list; + + const auto map{wm_listener_->getWorldModel()->getMap()}; + + if (map == nullptr) { + RCLCPP_WARN( + get_logger(), + "roadway_objects could not process external objects as no semantic map was available"); + + return; + } + + if (std::size(map->laneletLayer) == 0) { + RCLCPP_WARN( + get_logger(), + "roadway_objects could not process external objects as the semantic map does not contain any " + "lanelets"); + + return; + } + + const auto world_model{wm_listener_->getWorldModel()}; + for (auto object : msg.objects) { + const auto obstacle{world_model->toRoadwayObstacle(object)}; + + if (!obstacle.has_value()) { + RCLCPP_DEBUG_STREAM( + get_logger(), "roadway_objects dropping detected object with id: " + << object.id << " as it is off the road."); + + continue; + } + + obstacle_list.roadway_obstacles.emplace_back(obstacle.get()); + } + + roadway_obs_pub_->publish(obstacle_list); +} + +} // namespace roadway_objects + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(roadway_objects::RoadwayObjectsNode) diff --git a/roadway_objects/src/roadway_objects_node.cpp b/roadway_objects/src/roadway_objects_node.cpp index f58a4c870f..38e7898294 100644 --- a/roadway_objects/src/roadway_objects_node.cpp +++ b/roadway_objects/src/roadway_objects_node.cpp @@ -1,57 +1,33 @@ -/* - * Copyright (C) 2019-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 - * 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 "roadway_objects/roadway_objects_node.hpp" - -namespace roadway_objects +// Copyright 2019-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 +#include + +#include + +int main(int argc, char ** argv) { - namespace std_ph = std::placeholders; - - RoadwayObjectsNode::RoadwayObjectsNode(const rclcpp::NodeOptions &options) - : carma_ros2_utils::CarmaLifecycleNode(options) - { - } - - carma_ros2_utils::CallbackReturn RoadwayObjectsNode::handle_on_configure(const rclcpp_lifecycle::State &) - { - RCLCPP_INFO_STREAM(get_logger(), "RoadwayObjectsNode trying to configure"); - - wm_listener_ = std::make_shared(get_node_base_interface(), get_node_logging_interface(), - get_node_topics_interface(), get_node_parameters_interface()); - - object_worker_ = std::make_shared(wm_listener_->getWorldModel(), std::bind(&RoadwayObjectsNode::publishObstacles, this, std_ph::_1), get_node_logging_interface()); - - // Setup publishers - roadway_obs_pub_ = create_publisher("roadway_objects", 10); - - // Setup subscribers - external_objects_sub_ = create_subscription("external_objects", 10, - std::bind(&RoadwayObjectsWorker::externalObjectsCallback, object_worker_.get(), std_ph::_1)); - - // Return success if everthing initialized successfully - return CallbackReturn::SUCCESS; - } + rclcpp::init(argc, argv); - void RoadwayObjectsNode::publishObstacles(const carma_perception_msgs::msg::RoadwayObstacleList& msg) - { - roadway_obs_pub_->publish(msg); - } + auto node = std::make_shared(rclcpp::NodeOptions()); -} // roadway_objects + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); -#include "rclcpp_components/register_node_macro.hpp" + rclcpp::shutdown(); -// Register the component with class_loader -RCLCPP_COMPONENTS_REGISTER_NODE(roadway_objects::RoadwayObjectsNode) + return 0; +} diff --git a/roadway_objects/src/roadway_objects_worker.cpp b/roadway_objects/src/roadway_objects_worker.cpp deleted file mode 100644 index cffecc662f..0000000000 --- a/roadway_objects/src/roadway_objects_worker.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright (C) 2019-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 - * 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 "roadway_objects/roadway_objects_worker.hpp" - -namespace roadway_objects -{ -RoadwayObjectsWorker::RoadwayObjectsWorker(carma_wm::WorldModelConstPtr wm, const PublishObstaclesCallback& obj_pub, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) - : obj_pub_(obj_pub), wm_(wm), logger_(logger) -{ -} - -void RoadwayObjectsWorker::externalObjectsCallback(const carma_perception_msgs::msg::ExternalObjectList::UniquePtr obj_array) -{ - carma_perception_msgs::msg::RoadwayObstacleList obstacle_list; - auto map = wm_->getMap(); - if (!map) - { - RCLCPP_WARN_STREAM(logger_->get_logger(), "roadway_objects could not process external objects as no semantic map was available"); - return; - } - - if (map->laneletLayer.size() == 0) - { - RCLCPP_WARN_STREAM(logger_->get_logger(), "roadway_objects could not process external objects as the semantic map does not contain any lanelets"); - return; - } - - for (auto object : obj_array->objects) - { - lanelet::Optional obs = wm_->toRoadwayObstacle(object); - if (!obs) - { - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "roadway_objects dropping detected object with id: " << object.id << " as it is off the road."); - continue; - } - - obstacle_list.roadway_obstacles.emplace_back(obs.get()); - } - - obj_pub_(obstacle_list); - -} -} // namespace objects \ No newline at end of file diff --git a/roadway_objects/test/TestHelpers.hpp b/roadway_objects/test/TestHelpers.hpp deleted file mode 100644 index bac4667846..0000000000 --- a/roadway_objects/test/TestHelpers.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once -/* - * Copyright (C) 2019-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 - * 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 -#include -#include -#include -#include - -/** - * Helper file containing inline functions used to quickly build lanelet objects in unit tests - * - */ -namespace carma_wm -{ -inline lanelet::Point3d getPoint(double x, double y, double z) -{ - return lanelet::Point3d(lanelet::utils::getId(), x, y, z); -} - -inline lanelet::BasicPoint3d getBasicPoint(double x, double y, double z) -{ - return getPoint(x, y, z).basicPoint(); -} - -inline lanelet::BasicPoint2d getBasicPoint(double x, double y) -{ - return lanelet::utils::to2D(getPoint(x, y, 0.0)).basicPoint(); -} - -// Defaults to double solid line on left and double solid line on right -inline lanelet::Lanelet getLanelet(lanelet::LineString3d& left_ls, lanelet::LineString3d& right_ls, - const lanelet::Attribute& left_sub_type = lanelet::AttributeValueString::SolidSolid, - const lanelet::Attribute& right_sub_type = lanelet::AttributeValueString::Solid) -{ - left_ls.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::LineThin; - left_ls.attributes()[lanelet::AttributeName::Subtype] = left_sub_type; - - right_ls.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::LineThin; - right_ls.attributes()[lanelet::AttributeName::Subtype] = right_sub_type; - - lanelet::Lanelet ll; - ll.setId(lanelet::utils::getId()); - ll.setLeftBound(left_ls); - ll.setRightBound(right_ls); - - ll.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::Lanelet; - ll.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road; - ll.attributes()[lanelet::AttributeName::Location] = lanelet::AttributeValueString::Urban; - ll.attributes()[lanelet::AttributeName::OneWay] = "yes"; - ll.attributes()[lanelet::AttributeName::Dynamic] = "no"; - - return ll; -} - -inline lanelet::Lanelet getLanelet(std::vector left, std::vector right, - const lanelet::Attribute& left_sub_type = lanelet::AttributeValueString::SolidSolid, - const lanelet::Attribute& right_sub_type = lanelet::AttributeValueString::Solid) -{ - lanelet::LineString3d left_ls(lanelet::utils::getId(), left); - - lanelet::LineString3d right_ls(lanelet::utils::getId(), right); - - return getLanelet(left_ls, right_ls, left_sub_type, right_sub_type); -} -} // namespace carma_wm \ No newline at end of file diff --git a/roadway_objects/test/TestRoadwayObjectsWorker.cpp b/roadway_objects/test/TestRoadwayObjectsWorker.cpp deleted file mode 100644 index 0f56af7670..0000000000 --- a/roadway_objects/test/TestRoadwayObjectsWorker.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/* - * Copyright (C) 2019-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 - * 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 -#include -#include "TestHelpers.hpp" - -namespace roadway_objects -{ -TEST(RoadwayObjectsWorkerTest, testExternalObjectCallback) -{ - std::shared_ptr cmw = std::make_shared(); - - // Build map - auto p1 = carma_wm::getPoint(9, 0, 0); - auto p2 = carma_wm::getPoint(9, 9, 0); - auto p3 = carma_wm::getPoint(2, 0, 0); - auto p4 = carma_wm::getPoint(2, 9, 0); - lanelet::LineString3d right_ls_1(lanelet::utils::getId(), { p1, p2 }); - lanelet::LineString3d left_ls_1(lanelet::utils::getId(), { p3, p4 }); - auto ll_1 = carma_wm::getLanelet(left_ls_1, right_ls_1); - lanelet::LaneletMapPtr map = lanelet::utils::createMap({ ll_1 }, {}); - - // Build external object - geometry_msgs::msg::Pose pose; - pose.position.x = 6; - pose.position.y = 5; - pose.position.z = 0; - - tf2::Quaternion tf_orientation; - tf_orientation.setRPY(0, 0, 1.5708); - - pose.orientation.x = tf_orientation.getX(); - pose.orientation.y = tf_orientation.getY(); - pose.orientation.z = tf_orientation.getZ(); - pose.orientation.w = tf_orientation.getW(); - - geometry_msgs::msg::Vector3 size; - size.x = 4; - size.y = 2; - size.z = 1; - - carma_perception_msgs::msg::ExternalObject obj; - obj.id = 1; - obj.object_type = carma_perception_msgs::msg::ExternalObject::SMALL_VEHICLE; - obj.pose.pose = pose; - obj.velocity.twist.linear.x = 1.0; - - carma_perception_msgs::msg::PredictedState pred; - auto pred_pose = obj.pose.pose; - pred_pose.position.y += 1; - pred.predicted_position = pred_pose; - pred.predicted_position_confidence = 1.0; - - obj.predictions.push_back(pred); - - // Build roadway objects worker to test - carma_perception_msgs::msg::RoadwayObstacleList resulting_objs; - auto node = std::make_shared("test_node"); - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger = node->get_node_logging_interface(); - - RoadwayObjectsWorker row(std::static_pointer_cast(cmw), - [&](const carma_perception_msgs::msg::RoadwayObstacleList& objs) -> void { resulting_objs = objs; }, logger); - - ASSERT_EQ(resulting_objs.roadway_obstacles.size(), 0); // Verify resulting_objs is empty - - // Build external object list - carma_perception_msgs::msg::ExternalObjectList obj_list; - obj_list.objects.push_back(obj); - - // Test with no map set - std::unique_ptr obj_list_msg_ptr1 = std::make_unique(obj_list); - row.externalObjectsCallback(move(obj_list_msg_ptr1)); // Call function under test - - ASSERT_EQ(resulting_objs.roadway_obstacles.size(), 0); - - // Test with empty map - lanelet::LaneletMapPtr empty_map = lanelet::utils::createMap({}, {}); - - cmw->setMap(empty_map); - - std::unique_ptr obj_list_msg_ptr2 = std::make_unique(obj_list); - row.externalObjectsCallback(move(obj_list_msg_ptr2)); // Call function under test - - ASSERT_EQ(resulting_objs.roadway_obstacles.size(), 0); - - // Test with regular map - cmw->setMap(map); - - std::unique_ptr obj_list_msg_ptr3 = std::make_unique(obj_list); - row.externalObjectsCallback(move(obj_list_msg_ptr3)); // Call function under test - - ASSERT_EQ(resulting_objs.roadway_obstacles.size(), 1); - - carma_perception_msgs::msg::RoadwayObstacle obs = resulting_objs.roadway_obstacles[0]; - - ASSERT_EQ(obs.object.id, obj.id); // Check that the object was coppied - - ASSERT_EQ(obs.lanelet_id, ll_1.id()); - - ASSERT_EQ(obs.connected_vehicle_type.type, carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED); - - ASSERT_NEAR(obs.cross_track, 0.5, 0.00001); - - ASSERT_NEAR(obs.down_track, 5.0, 0.00001); - - ASSERT_EQ(obs.predicted_lanelet_ids.size(), 1); - ASSERT_EQ(obs.predicted_lanelet_ids[0], ll_1.id()); - - ASSERT_EQ(obs.predicted_lanelet_id_confidences.size(), 1); - ASSERT_NEAR(obs.predicted_lanelet_id_confidences[0], 0.9, 0.00001); - - ASSERT_EQ(obs.predicted_cross_tracks.size(), 1); - ASSERT_NEAR(obs.predicted_cross_tracks[0], 0.5, 0.00001); - - ASSERT_EQ(obs.predicted_cross_track_confidences.size(), 1); - ASSERT_NEAR(obs.predicted_cross_track_confidences[0], 0.9, 0.00001); - - ASSERT_EQ(obs.predicted_down_tracks.size(), 1); - ASSERT_NEAR(obs.predicted_down_tracks[0], 6.0, 0.00001); - - ASSERT_EQ(obs.predicted_down_track_confidences.size(), 1); - ASSERT_NEAR(obs.predicted_down_track_confidences[0], 0.9, 0.00001); -} - -} // namespace objects - -// Run all the tests -int main(int argc, char **argv) { - ::testing::InitGoogleTest(&argc, argv); - - //Initialize ROS - rclcpp::init(argc, argv); - - bool success = RUN_ALL_TESTS(); - - //shutdown ROS - rclcpp::shutdown(); - - return success; -} \ No newline at end of file diff --git a/roadway_objects/test/data/town01_vector_map_1.osm b/roadway_objects/test/data/town01_vector_map_1.osm new file mode 100644 index 0000000000..4629456465 --- /dev/null +++ b/roadway_objects/test/data/town01_vector_map_1.osm @@ -0,0 +1,55348 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roadway_objects/test/data/town01_vector_map_1_no_lanelets.osm b/roadway_objects/test/data/town01_vector_map_1_no_lanelets.osm new file mode 100644 index 0000000000..7db0dc940d --- /dev/null +++ b/roadway_objects/test/data/town01_vector_map_1_no_lanelets.osm @@ -0,0 +1,53620 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roadway_objects/test/roadway_objects_empty_map_launch_test.py b/roadway_objects/test/roadway_objects_empty_map_launch_test.py new file mode 100644 index 0000000000..0c45f3e1bf --- /dev/null +++ b/roadway_objects/test/roadway_objects_empty_map_launch_test.py @@ -0,0 +1,179 @@ +# Copyright 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. + +from pathlib import Path +import time +import unittest + +from ament_index_python import get_package_share_directory + +from carma_perception_msgs.msg import ExternalObjectList + +from launch import LaunchDescription +from launch.actions import TimerAction + +from launch_ros.actions import Node + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.srv import ChangeState +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +import rclpy.node +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.external_object_list_pub = self.create_publisher( + ExternalObjectList, "external_objects", 1 + ) + + +def try_transition_node( + node_name: str, transition_id: int, ros_context: Context +) -> bool: + transitioner = rclpy.create_node("transitioner", context=ros_context) + + srv_client = transitioner.create_client(ChangeState, f"{node_name}/change_state") + + if not srv_client.wait_for_service(timeout_sec=1.0): + return False + + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(transitioner) + + request = ChangeState.Request() + request.transition.id = transition_id + + future = srv_client.call_async(request) + executor.spin_once_until_future_complete(future) + + executor.remove_node(transitioner) + transitioner.destroy_node() + + return future.result().success + + +def publish_msg(node, publisher, msg, ros_context) -> None: + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(node) + + publisher.publish(msg) + + executor.remove_node(node) + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = Node( + package="roadway_objects", + executable="roadway_objects_node", + name="node_under_test", + parameters=[{"vehicle_participant_type": "vehicle:car"}], + ) + + lanelet2_map_path = ( + Path(get_package_share_directory("roadway_objects")) + / "test/data/town01_vector_map_1_no_lanelets.osm" + ) + + lanelet2_map_loader = Node( + package="map_file_ros2", + executable="lanelet2_map_loader_exec", + name="lanelet2_map_loader", + parameters=[{"lanelet2_filename": str(lanelet2_map_path)}], + remappings=[("lanelet_map_bin", "semantic_map")], + ) + + launch_description = LaunchDescription( + [ + node_under_test, + lanelet2_map_loader, + TimerAction(period=1.0, actions=[ReadyToTest()]), + ] + ) + + # These will get passed to the unittest test cases as keyword args + context = {"node_under_test": node_under_test} + + return launch_description, context + + +class TestEmptyMap(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_external_object_list_empty_map(self, proc_output, node_under_test): + trans_id = Transition.TRANSITION_CONFIGURE + self.assertTrue(try_transition_node("node_under_test", trans_id, self.context)) + + trans_id = Transition.TRANSITION_ACTIVATE + self.assertTrue(try_transition_node("node_under_test", trans_id, self.context)) + + trans_id = Transition.TRANSITION_CONFIGURE + self.assertTrue( + try_transition_node("lanelet2_map_loader", trans_id, self.context) + ) + + trans_id = Transition.TRANSITION_ACTIVATE + self.assertTrue( + try_transition_node("lanelet2_map_loader", trans_id, self.context) + ) + + # Need to wait for roadway_object's WMListener to build it routing graph + # Note: This value was arbitrarily chosen. It could probably be reduced. + time.sleep(5) + + external_object_list = ExternalObjectList() + publish_msg( + self.test_harness_node, + self.test_harness_node.external_object_list_pub, + external_object_list, + self.context, + ) + + proc_output.assertWaitFor( + "roadway_objects could not process external objects as the " + "semantic map does not contain any lanelets", + process=node_under_test, + timeout=1, + stream="stderr", + ) + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/roadway_objects/test/roadway_objects_no_map_launch_test.py b/roadway_objects/test/roadway_objects_no_map_launch_test.py new file mode 100644 index 0000000000..af77c9a822 --- /dev/null +++ b/roadway_objects/test/roadway_objects_no_map_launch_test.py @@ -0,0 +1,152 @@ +# Copyright 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. + +import time +import unittest + +from carma_perception_msgs.msg import ExternalObjectList + +from launch import LaunchDescription +from launch.actions import TimerAction + +from launch_ros.actions import Node + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.srv import ChangeState +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +import rclpy.node +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.external_object_list_pub = self.create_publisher( + ExternalObjectList, "external_objects", 1 + ) + + +def try_transition_node( + node_name: str, transition_id: int, ros_context: Context +) -> bool: + transitioner = rclpy.create_node("transitioner", context=ros_context) + + srv_client = transitioner.create_client(ChangeState, f"{node_name}/change_state") + + if not srv_client.wait_for_service(timeout_sec=1.0): + return False + + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(transitioner) + + request = ChangeState.Request() + request.transition.id = transition_id + + future = srv_client.call_async(request) + executor.spin_once_until_future_complete(future) + + executor.remove_node(transitioner) + transitioner.destroy_node() + + return future.result().success + + +def publish_msg(node, publisher, msg, ros_context) -> None: + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(node) + + publisher.publish(msg) + + executor.remove_node(node) + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = Node( + package="roadway_objects", + executable="roadway_objects_node", + name="node_under_test", + parameters=[{"vehicle_participant_type": "vehicle:car"}], + ) + + launch_description = LaunchDescription( + [ + node_under_test, + TimerAction(period=1.0, actions=[ReadyToTest()]), + ] + ) + + # These will get passed to the unittest test cases as keyword args + context = {"node_under_test": node_under_test} + + return launch_description, context + + +class TestEmptyMap(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_external_object_list_empty_map(self, proc_output, node_under_test): + trans_id = Transition.TRANSITION_CONFIGURE + self.assertTrue(try_transition_node("node_under_test", trans_id, self.context)) + + trans_id = Transition.TRANSITION_ACTIVATE + self.assertTrue(try_transition_node("node_under_test", trans_id, self.context)) + + # Need to wait for roadway_object's WMListener to build it routing graph + # Note: This value was arbitrarily chosen. It could probably be reduced. + time.sleep(5) + + external_object_list = ExternalObjectList() + publish_msg( + self.test_harness_node, + self.test_harness_node.external_object_list_pub, + external_object_list, + self.context, + ) + + proc_output.assertWaitFor( + "roadway_objects could not process external objects as no " + "semantic map was available", + process=node_under_test, + timeout=1, + stream="stderr", + ) + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/roadway_objects/test/roadway_objects_regular_map_launch_test.py b/roadway_objects/test/roadway_objects_regular_map_launch_test.py new file mode 100644 index 0000000000..919c5dcf67 --- /dev/null +++ b/roadway_objects/test/roadway_objects_regular_map_launch_test.py @@ -0,0 +1,302 @@ +# Copyright 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. + +import copy +from pathlib import Path +import time +import unittest +from typing import Sized + +from ament_index_python import get_package_share_directory + +from carma_perception_msgs.msg import ( + ConnectedVehicleType, + ExternalObject, + ExternalObjectList, + PredictedState, + RoadwayObstacleList, +) +from geometry_msgs.msg import Pose, Vector3 + +from launch import LaunchDescription +from launch.actions import TimerAction + +from launch_ros.actions import Node + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.srv import ChangeState +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +import rclpy.node +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.external_object_list_pub = self.create_publisher( + ExternalObjectList, "external_objects", 1 + ) + + self.roadway_obstacle_list_sub = self.create_subscription( + RoadwayObstacleList, + "roadway_objects", + self.store_msg, + 1, + ) + + self.roadway_obstacle_lists = [] + + def store_msg(self, msg) -> None: + self.roadway_obstacle_lists.append(msg) + + +def make_external_object_list() -> ExternalObjectList: + external_object = ExternalObject() + external_object.id = 1 + external_object.object_type = ExternalObject.SMALL_VEHICLE + external_object.header.frame_id = "map" + + pose = Pose() + pose.position.x = 45.8529 + pose.position.y = -223.004 + pose.position.z = 0.0 + pose.orientation.x = 0.0 + pose.orientation.y = 0.0 + pose.orientation.z = 0.7071081 + pose.orientation.w = 0.7071055 + + external_object.pose.pose = pose + + external_object.velocity.twist.linear.x = 1.0 + + size = Vector3() + size.x = 4.0 + size.y = 2.0 + size.z = 1.0 + + external_object.size = size + + predicted_state = PredictedState() + predicted_state.predicted_position = copy.deepcopy(pose) + predicted_state.predicted_position.position.y += 1.0 + predicted_state.predicted_position_confidence = 1.0 + + external_object.predictions.append(predicted_state) + + external_object_list = ExternalObjectList() + external_object_list.objects.append(external_object) + + return external_object_list + + +def try_transition_node( + node_name: str, transition_id: int, ros_context: Context +) -> bool: + transitioner = rclpy.create_node("transitioner", context=ros_context) + + srv_client = transitioner.create_client(ChangeState, f"{node_name}/change_state") + + if not srv_client.wait_for_service(timeout_sec=1.0): + return False + + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(transitioner) + + request = ChangeState.Request() + request.transition.id = transition_id + + future = srv_client.call_async(request) + executor.spin_once_until_future_complete(future) + + executor.remove_node(transitioner) + transitioner.destroy_node() + + return future.result().success + + +def publish_msg(node, publisher, msg, ros_context) -> None: + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(node) + + publisher.publish(msg) + + executor.remove_node(node) + + +def spin_node_until(node, condition, ros_context, timeout_sec=60.0) -> None: + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(node) + + end_time = time.time() + timeout_sec + while not condition() and time.time() < end_time: + executor.spin_once(timeout_sec=0.1) + + executor.remove_node(node) + + +class LenIncreases: + def __init__(self, sized_object: Sized) -> None: + self.sized_object = sized_object + self.original_size = len(sized_object) + + def __call__(self) -> bool: + return len(self.sized_object) > self.original_size + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = Node( + package="roadway_objects", + executable="roadway_objects_node", + name="node_under_test", + parameters=[{"vehicle_participant_type": "vehicle:car"}], + ) + + lanelet2_map_path = ( + Path(get_package_share_directory("roadway_objects")) + / "test/data/town01_vector_map_1.osm" + ) + + lanelet2_map_loader = Node( + package="map_file_ros2", + executable="lanelet2_map_loader_exec", + name="lanelet2_map_loader", + parameters=[{"lanelet2_filename": str(lanelet2_map_path)}], + remappings=[("lanelet_map_bin", "semantic_map")], + ) + + launch_description = LaunchDescription( + [ + node_under_test, + lanelet2_map_loader, + TimerAction(period=1.0, actions=[ReadyToTest()]), + ] + ) + + # These will get passed to the unittest test cases as keyword args + context = {"node_under_test": node_under_test} + + return launch_description, context + + +class TestRegularMap(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_external_object_list_regular_map(self): + trans_id = Transition.TRANSITION_CONFIGURE + self.assertTrue(try_transition_node("node_under_test", trans_id, self.context)) + + trans_id = Transition.TRANSITION_ACTIVATE + self.assertTrue(try_transition_node("node_under_test", trans_id, self.context)) + + trans_id = Transition.TRANSITION_CONFIGURE + self.assertTrue( + try_transition_node("lanelet2_map_loader", trans_id, self.context) + ) + + trans_id = Transition.TRANSITION_ACTIVATE + self.assertTrue( + try_transition_node("lanelet2_map_loader", trans_id, self.context) + ) + + # Need to wait for roadway_object's WMListener to build it routing graph + # Note: This value was arbitrarily chosen. It could probably be reduced. + time.sleep(5) + + external_object_list = make_external_object_list() + publish_msg( + self.test_harness_node, + self.test_harness_node.external_object_list_pub, + external_object_list, + self.context, + ) + + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.roadway_obstacle_lists), + self.context, + ) + + self.assertTrue(len(self.test_harness_node.roadway_obstacle_lists) > 0) + roadway_obstacle_list = self.test_harness_node.roadway_obstacle_lists[-1] + + external_object = external_object_list.objects[0] + roadway_obstacle = roadway_obstacle_list.roadway_obstacles[0] + + self.assertEqual(external_object, roadway_obstacle.object) + + self.assertEqual(roadway_obstacle.lanelet_id, 144) + self.assertEqual( + roadway_obstacle.connected_vehicle_type.type, + ConnectedVehicleType.NOT_CONNECTED, + ) + + self.assertAlmostEqual(roadway_obstacle.cross_track, 0.0, delta=1e-4) + self.assertAlmostEqual(roadway_obstacle.down_track, 8.308, delta=1e-3) + + self.assertEqual(len(roadway_obstacle.predicted_lanelet_ids), 1) + self.assertEqual(roadway_obstacle.predicted_lanelet_ids[0], 144) + + self.assertEqual(len(roadway_obstacle.predicted_lanelet_id_confidences), 1) + self.assertAlmostEqual( + roadway_obstacle.predicted_lanelet_id_confidences[0], 0.9 + ) + + self.assertEqual(len(roadway_obstacle.predicted_cross_tracks), 1) + self.assertAlmostEqual( + roadway_obstacle.predicted_cross_tracks[0], 0.0, delta=1e-4 + ) + + self.assertEqual(len(roadway_obstacle.predicted_cross_track_confidences), 1) + self.assertAlmostEqual( + roadway_obstacle.predicted_cross_track_confidences[0], 0.9 + ) + + self.assertEqual(len(roadway_obstacle.predicted_down_tracks), 1) + self.assertAlmostEqual( + roadway_obstacle.predicted_down_tracks[0], 9.308, delta=1e-3 + ) + + self.assertEqual(len(roadway_obstacle.predicted_down_track_confidences), 1) + self.assertAlmostEqual( + roadway_obstacle.predicted_down_track_confidences[0], 0.9 + ) + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info) diff --git a/route/src/route_node.cpp b/route/src/route_node.cpp index c2d5900bcb..6d81d52b84 100644 --- a/route/src/route_node.cpp +++ b/route/src/route_node.cpp @@ -81,7 +81,17 @@ namespace route // Setup publishers route_pub_ = create_publisher("route", 1); route_state_pub_ = create_publisher("route_state", 1); - route_event_pub_ = create_publisher("route_event", 1); + + // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic + pub_qos_transient_local.transient_local(); // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers + // NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner. + + route_event_pub_ = create_publisher("route_event", pub_qos_transient_local, intra_proc_disabled); route_marker_pub_ = create_publisher("route_marker", 1); // Setup service servers diff --git a/subsystem_controllers/CMakeLists.txt b/subsystem_controllers/CMakeLists.txt index 74bb2ed31e..1a7bf52600 100644 --- a/subsystem_controllers/CMakeLists.txt +++ b/subsystem_controllers/CMakeLists.txt @@ -55,7 +55,11 @@ ament_auto_add_executable(environment_perception_controller src/environment_perc target_link_libraries(environment_perception_controller environment_perception_controller_core ${base_lib}) # Driver Subsystem -ament_auto_add_library(drivers_controller_core SHARED src/drivers_controller/drivers_controller_node.cpp) +ament_auto_add_library(drivers_controller_core SHARED + src/drivers_controller/drivers_controller_node.cpp + src/drivers_controller/entry_manager.cpp + src/drivers_controller/driver_manager.cpp + ) rclcpp_components_register_nodes(drivers_controller_core "subsystem_controllers::DriversControllerNode") target_link_libraries(drivers_controller_core ${base_lib}) ament_auto_add_executable(drivers_controller src/drivers_controller/main.cpp) diff --git a/subsystem_controllers/config/drivers_controller_config.yaml b/subsystem_controllers/config/drivers_controller_config.yaml index bec8d7f859..185d29c9c0 100644 --- a/subsystem_controllers/config/drivers_controller_config.yaml +++ b/subsystem_controllers/config/drivers_controller_config.yaml @@ -12,12 +12,27 @@ subsystem_namespace: /hardware_interface # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed - # TODO Move this parameter to carma config in some capacity required_subsystem_nodes: [''] # List of nodes which will not be directly managed by this subsystem controller # but which are required to be operational for the subsystem to function unmanaged_required_nodes: [''] + # List of nodes in the namespace which will not be managed by this subsystem controller + # Specifically includes the lidar and gps nodes which are handled in other subsystem controllers + excluded_namespace_nodes : [''] + + # List of ros1 controller drivers (node name) to consider required and who's failure shall result in automation abort + ros1_required_drivers: [''] + + # List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort. + ros1_camera_drivers: [''] + # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes - full_subsystem_required: false \ No newline at end of file + full_subsystem_required: false + + # Int: The time allocated for system startup in seconds + startup_duration: 30 + + # Double: The timeout threshold for essential drivers in ms + required_driver_timeout: 3000.0 \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp new file mode 100644 index 0000000000..1cdd9dca9f --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp @@ -0,0 +1,104 @@ +#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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "entry_manager.hpp" +#include "entry.hpp" +#include +#include +#include + + +namespace subsystem_controllers +{ + + /** + * \brief The DriverManager serves as a component to manage CARMA required ROS1 Drivers + */ + class DriverManager + { + public: + + /*! + * \brief Default constructor for DriverManager with driver_timeout_ = 1000ms + */ + DriverManager(); + + /** + * \brief Constructor for DriverManager + * + * \param critical_driver_names The set of drivers which will be treated as required. A failure in these plugins will result in an exception + * \param camera_entries The set of camera drivers. + * \param driver_timeout The timeout threshold for essential drivers + */ + DriverManager(const std::vector& critical_driver_names, + const std::vector& camera_entries, + const long driver_timeout); + + + /*! + * \brief Update driver status + */ + void update_driver_status(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg, long current_time); + + /*! + * \brief Check if all critical drivers are operational + */ + std::string are_critical_drivers_operational(long current_time); + + /*! + * \brief Evaluate if the sensor is available + */ + void evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout); + + /*! + * \brief Handle the spin and publisher + */ + carma_msgs::msg::SystemAlert handle_spin(long time_now,long start_up_timestamp,long startup_duration); + + protected: + + //list of critical drivers + std::vector critical_drivers_; + + //list of camera entries + std::vector camera_entries_; + + //! Entry manager to keep track of detected plugins + std::shared_ptr em_; + + // timeout for critical driver timeout in milliseconds + long driver_timeout_ = 1000; + + bool starting_up_ = true; + + FRIEND_TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown); + + }; +} \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp new file mode 100644 index 0000000000..fa07914fbb --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp @@ -0,0 +1,71 @@ +#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 +#include + +namespace subsystem_controllers +{ + /** + * \brief Stuct containing the algorithm configuration values for the GuidanceController + */ + struct DriversControllerConfig + + { + //! List of ros1 controller drivers (node name) to consider required and who's failure shall result in automation abort. + std::vector ros1_required_drivers_; + //! List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort. + std::vector ros1_camera_drivers_; + //! List of nodes in the namespace which will not be managed by this subsystem controller + std::vector excluded_namespace_nodes_; + //! The time allocated for system startup in seconds + int startup_duration_; + //! The timeout threshold for essential drivers in ms + double driver_timeout_ = 1000; + + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const DriversControllerConfig &c) + { + + output << "DriversControllerConfig { " << std::endl + << "ros1_required_drivers: [ " << std::endl; + + for (auto node : c.ros1_required_drivers_) + output << node << " "; + + output << "] " << std::endl << "ros1_camera_drivers: [ "; + + for (auto node : c.ros1_camera_drivers_) + output << node << " "; + + output << "] " << std::endl << "excluded_namespace_nodes: [ "; + + for (auto node : c.excluded_namespace_nodes_) + output << node << " "; + + output<< "] " << std::endl << "startup_duration: "<< c.startup_duration_ << std::endl; + + output <<"driver_timeout: "<< c.driver_timeout_ << std::endl + + << "}" << std::endl; + return output; + } + }; + +} // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp index fd64001cb3..349a4602c5 100644 --- a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp @@ -23,7 +23,11 @@ #include "carma_msgs/msg/system_alert.hpp" #include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp" #include "rclcpp/rclcpp.hpp" +#include #include "subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp" +#include "drivers_controller_config.hpp" +#include "driver_manager.hpp" +#include namespace subsystem_controllers { @@ -43,8 +47,38 @@ namespace subsystem_controllers */ explicit DriversControllerNode(const rclcpp::NodeOptions &options); - // TODO integrate driver_discovery/health_monitor behavior into this node - // https://github.com/usdot-fhwa-stol/carma-platform/issues/1500 + + private: + + // DriverManager to handle all the driver specific discovery and reporting + std::shared_ptr driver_manager_; + + //! Config for user provided parameters + DriversControllerConfig config_; + + //! ROS handles + carma_ros2_utils::SubPtr driver_status_sub_; + + // message/service callbacks + void driver_discovery_cb(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg); + + //! Timer callback function to check status of required ros1 drivers + void timer_callback(); + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &prev_state); + + //! ROS parameters + std::vector ros1_required_drivers_; + std::vector ros1_camera_drivers_; + + // record of startup timestamp + long start_up_timestamp_; + + rclcpp::TimerBase::SharedPtr timer_; + + // Previously published alert message + boost::optional prev_alert; }; diff --git a/health_monitor/include/entry.h b/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry.hpp similarity index 53% rename from health_monitor/include/entry.h rename to subsystem_controllers/include/subsystem_controllers/drivers_controller/entry.hpp index f5c5a38861..8808937119 100644 --- a/health_monitor/include/entry.h +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2019-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 @@ -18,18 +18,27 @@ #include -namespace health_monitor +namespace subsystem_controllers { + /** + * \brief An entry represents a driver details for the purposes of tracking + */ struct Entry { - /* data */ - bool available_; - bool active_; + //! Availability flag of a driver + bool available_ = false; + //! Fully specified node name of a driver std::string name_; + //! The timestamp at which the entry was last updated long timestamp_; - uint8_t type_; - std::string capability_; - Entry(bool available, bool active, const std::string& name, long timestamp, uint8_t type, const std::string& capability); + /** + * \brief All fields constructor + */ + Entry(bool available, const std::string& name, long timestamp) + : available_(available), name_(name), timestamp_(timestamp) {} + + + Entry() = default; }; } \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp new file mode 100644 index 0000000000..46a3f924ae --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp @@ -0,0 +1,94 @@ +#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 +#include +#include +#include "entry.hpp" + + +namespace subsystem_controllers +{ + /** + * \brief The EntryManager serves as a component to track the status of each required CARMA ROS 1 driver + */ + class EntryManager + { + + public: + + /*! + * \brief Default constructor for EntryManager. + */ + EntryManager(); + /*! + * \brief Constructor for EntryManager to set required entries. + */ + EntryManager(std::vector required_entries); + + /*! + * \brief Constructor for EntryManager to set required entries and camera entires. + */ + EntryManager(std::vector required_entries, std::vector camera_entries); + + /*! + * \brief Add a new entry if the given name does not exist. + * Update an existing entry if the given name exists. + */ + void update_entry(const Entry& entry); + + /*! + * \brief Get all registed entries as a list. + */ + std::vector get_entries() const; + + /*! + * \brief Get a entry using name as the key. + */ + boost::optional get_entry_by_name(const std::string& name) const; + + /*! + * \brief Delete an entry using the given name as the key. + */ + void delete_entry(const std::string& name); + + /*! + * \brief Check if the entry is required + */ + bool is_entry_required(const std::string& name) const; + + /*! + * \brief Check if the entry is a required camera entry + */ + int is_camera_entry_required(const std::string& name) const; + + private: + + //! private list to keep track of all entries + std::vector entry_list_; + + //list of required entries + std::vector required_entries_; + + //list of camera entries + std::vector camera_entries_; + + }; + + +} \ No newline at end of file diff --git a/subsystem_controllers/package.xml b/subsystem_controllers/package.xml index d234e2f38a..102b366fd0 100644 --- a/subsystem_controllers/package.xml +++ b/subsystem_controllers/package.xml @@ -29,6 +29,7 @@ carma_cmake_common carma_ros2_utils carma_msgs + carma_driver_msgs carma_planning_msgs lifecycle_msgs rclcpp diff --git a/subsystem_controllers/src/drivers_controller/driver_manager.cpp b/subsystem_controllers/src/drivers_controller/driver_manager.cpp new file mode 100644 index 0000000000..2819f98be4 --- /dev/null +++ b/subsystem_controllers/src/drivers_controller/driver_manager.cpp @@ -0,0 +1,150 @@ +/* + * 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 +#include +#include +#include +#include "subsystem_controllers/drivers_controller/driver_manager.hpp" + + +using std_msec = std::chrono::milliseconds; + +namespace subsystem_controllers +{ + DriverManager::DriverManager() {} + + DriverManager::DriverManager(const std::vector& critical_driver_names, + const std::vector& camera_entries, + const long driver_timeout) + : critical_drivers_(critical_driver_names.begin(), critical_driver_names.end()), + camera_entries_(camera_entries.begin(), camera_entries.end()), + driver_timeout_(driver_timeout) + { + // Intialize entry manager + em_ = std::make_shared(critical_driver_names, camera_entries); + + } + + + carma_msgs::msg::SystemAlert DriverManager::handle_spin(long time_now,long start_up_timestamp,long startup_duration) + { + carma_msgs::msg::SystemAlert alert; + + std::string status = are_critical_drivers_operational(time_now); + if(status.compare("s_1_c_1") == 0) + { + starting_up_ = false; + alert.description = "All essential drivers are ready"; + alert.type = carma_msgs::msg::SystemAlert::DRIVERS_READY; + return alert; + } + else if(starting_up_ && (time_now - start_up_timestamp <= startup_duration)) + { + alert.description = "System is starting up..."; + alert.type = carma_msgs::msg::SystemAlert::NOT_READY; + return alert; + } + else if(status.compare("s_1_c_0")==0) + { + alert.description = "Camera Failed"; + alert.type = carma_msgs::msg::SystemAlert::SHUTDOWN; + return alert; + } + else if(status.compare("s_0") == 0) + { + alert.description = "SSC Failed"; + alert.type = carma_msgs::msg::SystemAlert::SHUTDOWN; + return alert; + } + else + { + alert.description = "Unknown problem assessing essential driver availability"; + alert.type = carma_msgs::msg::SystemAlert::FATAL; + return alert; + } + + } + + + void DriverManager::update_driver_status(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg, long current_time) + { + // update driver status is only called in response to a message received on driver_discovery. This topic is only being published in ros1 + Entry driver_status(msg->status == carma_driver_msgs::msg::DriverStatus::OPERATIONAL || msg->status == carma_driver_msgs::msg::DriverStatus::DEGRADED, + msg->name,current_time); + + em_->update_entry(driver_status); + } + + + void DriverManager::evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout) + { + + if((!available) || (current_time-timestamp > driver_timeout)) + { + sensor_input=0; + } + else + { + sensor_input=1; + } + + } + + std::string DriverManager::are_critical_drivers_operational(long current_time) + { + int ssc=0; + int camera=0; + + std::vector driver_list = em_->get_entries(); //Real time driver list from driver status + for(auto i = driver_list.begin(); i < driver_list.end(); ++i) + { + if(em_->is_entry_required(i->name_)) + { + evaluate_sensor(ssc,i->available_,current_time,i->timestamp_,driver_timeout_); + } + else if(em_->is_camera_entry_required(i->name_)==0) + { + evaluate_sensor(camera,i->available_,current_time,i->timestamp_,driver_timeout_); + } + } + + // Manual disable of ssc entry in case ssc wrapper is in ros2 + if (critical_drivers_.empty()) + { + ssc = 1; + } + + ///////////////////// + //Decision making + if (ssc == 1 && camera == 1) + { + return "s_1_c_1"; + } + else if (ssc == 1 && camera == 0) + { + return "s_1_c_0"; + } + else{ + return "s_0"; + } + + + } + + +} \ No newline at end of file diff --git a/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp b/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp index 4202453ea0..f74c76ef24 100644 --- a/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp +++ b/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp @@ -22,8 +22,136 @@ namespace subsystem_controllers DriversControllerNode::DriversControllerNode(const rclcpp::NodeOptions &options) : BaseSubsystemController(options) { + // Don't automatically trigger state transitions from base class on configure + // In this class the managed nodes list first needs to be modified then the transition will be triggered manually + trigger_managed_nodes_configure_from_base_class_ = false; + + config_.startup_duration_ = declare_parameter("startup_duration", config_.startup_duration_); + config_.driver_timeout_ = declare_parameter("required_driver_timeout", config_.driver_timeout_); + + // carma-config parameters + config_.ros1_required_drivers_ = declare_parameter>("ros1_required_drivers", config_.ros1_required_drivers_); + config_.ros1_camera_drivers_ = declare_parameter>("ros1_camera_drivers", config_.ros1_camera_drivers_); + config_.excluded_namespace_nodes_ = declare_parameter>("excluded_namespace_nodes", config_.excluded_namespace_nodes_); + } + carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + auto base_return = BaseSubsystemController::handle_on_configure(prev_state); + + if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Drivers Controller could not configure"); + return base_return; + } + + config_ = DriversControllerConfig(); + + // Load required plugins and default enabled plugins + get_parameter>("ros1_required_drivers", config_.ros1_required_drivers_); + get_parameter>("ros1_camera_drivers", config_.ros1_camera_drivers_); + get_parameter("startup_duration", config_.startup_duration_); + get_parameter("required_driver_timeout", config_.driver_timeout_); + get_parameter>("excluded_namespace_nodes", config_.excluded_namespace_nodes_); + + RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); + + // Handle fact that parameter vectors cannot be empty + if (config_.ros1_required_drivers_.size() == 1 && config_.ros1_required_drivers_[0].empty()) { + config_.ros1_required_drivers_.clear(); + } + if (config_.ros1_camera_drivers_.size() == 1 && config_.ros1_camera_drivers_[0].empty()) { + config_.ros1_camera_drivers_.clear(); + } + if (config_.excluded_namespace_nodes_.size() == 1 && config_.excluded_namespace_nodes_[0].empty()) { + config_.excluded_namespace_nodes_.clear(); + } + + auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes(); + // Update managed nodes + // Collect namespace nodes not managed by other subsystem controllers - manually specified in carma-config + auto updated_managed_nodes = get_non_intersecting_set(base_managed_nodes, config_.excluded_namespace_nodes_); + + lifecycle_mgr_.set_managed_nodes(updated_managed_nodes); + + driver_manager_ = std::make_shared( + config_.ros1_required_drivers_, + config_.ros1_camera_drivers_, + config_.driver_timeout_ + ); + + // record starup time + start_up_timestamp_ = this->now().nanoseconds() / 1e6; + + driver_status_sub_ = create_subscription("driver_discovery", 1, std::bind(&DriversControllerNode::driver_discovery_cb, this, std::placeholders::_1)); + + timer_ = create_timer(get_clock(), std::chrono::milliseconds(1000), std::bind(&DriversControllerNode::timer_callback,this)); + + // Configure our drivers + bool success = lifecycle_mgr_.configure(std::chrono::milliseconds(base_config_.service_timeout_ms), std::chrono::milliseconds(base_config_.call_timeout_ms)).empty(); + + if (success) + { + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to configure"); + return CallbackReturn::SUCCESS; + } + else + { + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to configure"); + return CallbackReturn::FAILURE; + } + + } + + carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + + // Reset to automatically trigger state transitions from base class + trigger_managed_nodes_configure_from_base_class_ = true; + + auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all base_managed_nodes + + if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Driver Controller could not activate"); + return base_return; + } + + } + + void DriversControllerNode::timer_callback() + { + + long time_now = this->now().nanoseconds() / 1e6; + rclcpp::Duration sd(config_.startup_duration_, 0); + long start_duration = sd.nanoseconds()/1e6; + + auto dm = driver_manager_->handle_spin(time_now, start_up_timestamp_, start_duration); + + //Wait for node to be activated + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE){ + if (!prev_alert) { + prev_alert = dm; + publish_system_alert(dm); + } + else if ( prev_alert->type == dm.type && prev_alert->description.compare(dm.description) == 0) { // Do not publish duplicate alerts + RCLCPP_DEBUG_STREAM(get_logger(), "No change to alert status"); + } + else{ + prev_alert = dm; + publish_system_alert(dm); + } + } + + } + + + void DriversControllerNode::driver_discovery_cb(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg) + { + // Driver discovery only published by ros1 drivers + driver_manager_->update_driver_status(msg, this->now().nanoseconds()/1e6); + } + + } // namespace subsystem_controllers #include "rclcpp_components/register_node_macro.hpp" diff --git a/health_monitor/src/entry_manager.cpp b/subsystem_controllers/src/drivers_controller/entry_manager.cpp similarity index 75% rename from health_monitor/src/entry_manager.cpp rename to subsystem_controllers/src/drivers_controller/entry_manager.cpp index 14b9912fad..02bdce3da0 100644 --- a/health_monitor/src/entry_manager.cpp +++ b/subsystem_controllers/src/drivers_controller/entry_manager.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-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 @@ -14,18 +14,17 @@ * the License. */ -#include "entry_manager.h" +#include "subsystem_controllers/drivers_controller/entry_manager.hpp" #include -namespace health_monitor +namespace subsystem_controllers { - EntryManager::EntryManager() {} - EntryManager::EntryManager(std::vector required_entries):required_entries_(required_entries) {} - - EntryManager::EntryManager(std::vector required_entries,std::vector lidar_gps_entries, - std::vector camera_entries) :required_entries_(required_entries),lidar_gps_entries_(lidar_gps_entries), camera_entries_(camera_entries){} + EntryManager::EntryManager(std::vector required_entries):required_entries_(required_entries) {} + + EntryManager::EntryManager(std::vector required_entries, std::vector camera_entries) + :required_entries_(required_entries), camera_entries_(camera_entries){} void EntryManager::update_entry(const Entry& entry) { @@ -33,8 +32,7 @@ namespace health_monitor { if(i->name_.compare(entry.name_) == 0) { - // name and type of the entry wont change - i->active_ = entry.active_; + // name entry wont change i->available_ = entry.available_; i->timestamp_ = entry.timestamp_; return; @@ -43,7 +41,6 @@ namespace health_monitor entry_list_.push_back(entry); } - std::vector EntryManager::get_entries() const { // returns the copy of the original list @@ -75,7 +72,7 @@ namespace health_monitor return boost::none; } - bool EntryManager::is_entry_required(const std::string& name) const + bool EntryManager::is_entry_required(const std::string& name) const { for(auto i = required_entries_.begin(); i < required_entries_.end(); ++i) { @@ -87,21 +84,6 @@ namespace health_monitor return false; } - int EntryManager::is_lidar_gps_entry_required(const std::string& name) const - { - - for(int i=0;i +#include + + +namespace subsystem_controllers +{ + TEST(DriverManagerTest, testCarNormalDriverStatus) + { + // inordinary case where no critical drivers are specified + DriverManager dm0({"ssc"}, {}, 0); + carma_driver_msgs::msg::DriverStatus msg0; + msg0.controller = true; + msg0.name = "controller"; + msg0.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + + auto msg0_pointer = std::make_shared(msg0); + + dm0.update_driver_status(msg0_pointer, 1000); + + EXPECT_EQ("s_0", dm0.are_critical_drivers_operational(1500)); + + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers, camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + EXPECT_EQ("s_1_c_1", dm.are_critical_drivers_operational(1500)); + } + + TEST(DriverManagerTest, testSsc_1_Camera_0) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + + DriverManager dm(required_drivers,camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + EXPECT_EQ("s_1_c_0", dm.are_critical_drivers_operational(1500)); + } + + TEST(DriverManagerTest, testCarErrorDriverStatusTimeOut) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers, camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + EXPECT_EQ("s_0", dm.are_critical_drivers_operational(2100)); + } + + TEST(DriverManagerTest, testCarHandleSpinDriversReady) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers,camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert=dm.handle_spin(1500,150,750); + + EXPECT_EQ(5, alert.type); + } + + TEST(DriverManagerTest, testCarHandleSpinFatalSscWorking) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers, camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + + carma_msgs::msg::SystemAlert alert; + alert=dm.handle_spin(1500,150,750); + + EXPECT_EQ(6, alert.type); + } + + TEST(DriverManagerTest, testCarHandleSpinFatalUnknownInside) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers,camera_drivers, 1000L); + + carma_msgs::msg::SystemAlert alert; + alert=dm.handle_spin(1500,150,750); + + EXPECT_EQ(6, alert.type); + } + + + TEST(DriverManagerTest, testCarHandleSpinNotReadyCase1) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers,camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + + carma_msgs::msg::SystemAlert alert; + alert=dm.handle_spin(1500,1000,750); + + EXPECT_EQ(4, alert.type); + } + + TEST(DriverManagerTest, testCarHandleSpinNotReadyCase2) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers,camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert=dm.handle_spin(1500,5000,750); + + EXPECT_EQ(4, alert.type); + } + + TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown) + { + std::vector required_drivers{"controller"}; + std::vector camera_drivers{"camera"}; + + DriverManager dm(required_drivers,camera_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_driver_msgs::msg::DriverStatus msg2; + msg2.gnss = true; + msg2.name = "camera"; + msg2.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); + dm.update_driver_status(msg2_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert=dm.handle_spin(1500,150,750); + + EXPECT_EQ(6, alert.type); + } +} \ No newline at end of file diff --git a/health_monitor/test/test_entry_manager.cpp b/subsystem_controllers/test/test_driver_subsystem/test_entry_manager.cpp similarity index 54% rename from health_monitor/test/test_entry_manager.cpp rename to subsystem_controllers/test/test_driver_subsystem/test_entry_manager.cpp index 6837117756..7568037eba 100644 --- a/health_monitor/test/test_entry_manager.cpp +++ b/subsystem_controllers/test/test_driver_subsystem/test_entry_manager.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-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 @@ -14,84 +14,79 @@ * the License. */ -#include "entry_manager.h" +#include #include #include -namespace health_monitor +namespace subsystem_controllers { - - TEST(EntryManagerTest, testAddNewEntry) + TEST(EntryManagerTest, testNewEntry) { + // Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability, bool is_ros1) EntryManager em; // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + Entry entry(true,"cruising_plugin", 1000); em.update_entry(entry); + + std::vector res = em.get_entries(); EXPECT_EQ(1, res.size()); EXPECT_EQ(true, res.begin()->available_); - EXPECT_EQ(false, res.begin()->active_); EXPECT_EQ("cruising_plugin", res.begin()->name_); EXPECT_EQ(1000, res.begin()->timestamp_); - EXPECT_EQ(1, res.begin()->type_); + } - TEST(EntryManagerTest, testUpdateEntry) + TEST(EntryManagerTest, testUpdatedEntry) { EntryManager em; // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + Entry entry(true, "cruising_plugin", 1000); em.update_entry(entry); - Entry new_entry(false, true, "cruising_plugin", 2000, 3, ""); + Entry new_entry(false, "cruising_plugin", 2000); em.update_entry(new_entry); std::vector res = em.get_entries(); EXPECT_EQ(1, res.size()); EXPECT_EQ(false, res.begin()->available_); - EXPECT_EQ(true, res.begin()->active_); EXPECT_EQ(2000, res.begin()->timestamp_); // the following two attributes should not change once set EXPECT_EQ("cruising_plugin", res.begin()->name_); - EXPECT_EQ(1, res.begin()->type_); } - TEST(EntryManagerTest, testDeleteEntry) + TEST(EntryManagerTest, testDeletedEntry) { EntryManager em; // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + Entry entry(true, "cruising_plugin",1000); em.update_entry(entry); - Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + Entry new_entry(false, "cruising_plugin_2", 2000); em.update_entry(new_entry); em.delete_entry("cruising_plugin"); std::vector res = em.get_entries(); EXPECT_EQ(1, res.size()); EXPECT_EQ(false, res.begin()->available_); - EXPECT_EQ(true, res.begin()->active_); EXPECT_EQ(2000, res.begin()->timestamp_); // the following two attributes should not change once set EXPECT_EQ("cruising_plugin_2", res.begin()->name_); - EXPECT_EQ(3, res.begin()->type_); } - TEST(EntryManagerTest, testGetEntryByValidName) + TEST(EntryManagerTest, testGetEntryWithValidName) { EntryManager em; // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + Entry entry(true, "cruising_plugin", 1000); em.update_entry(entry); boost::optional res = em.get_entry_by_name("cruising_plugin"); EXPECT_EQ(true, res->available_); - EXPECT_EQ(false, res->active_); EXPECT_EQ(1000, res->timestamp_); - EXPECT_EQ(1, res->type_); EXPECT_EQ("cruising_plugin", res->name_); } - TEST(EntryManagerTest, testGetEntryByInvalidName) + TEST(EntryManagerTest, testGetEntryWithInvalidName) { EntryManager em; // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + Entry entry(true, "cruising_plugin", true); em.update_entry(entry); boost::optional res = em.get_entry_by_name("plugin"); if(!res) @@ -103,60 +98,19 @@ namespace health_monitor } } - TEST(EntryManagerTest, testRequiredEntryCheck) + TEST(EntryManagerTest, testRequiredEntry) { std::vector required_names; required_names.push_back("cruising"); required_names.push_back("cruising_plugin"); EntryManager em(required_names); - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + Entry entry(true, "cruising_plugin", 1000); em.update_entry(entry); - Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + Entry new_entry(false, "cruising_plugin_2", 2000); em.update_entry(new_entry); EXPECT_EQ(true, em.is_entry_required("cruising_plugin")); EXPECT_EQ(true, em.is_entry_required("cruising")); EXPECT_EQ(false, em.is_entry_required("cruising_plugin_2")); } - TEST(EntryManagerTest, testTruckLidarGpsEntryCheck) - { - std::vector required_entries; - required_entries.push_back("ssc"); - - std::vector lidar_gps_entries; - lidar_gps_entries.push_back("lidar1"); - lidar_gps_entries.push_back("lidar2"); - lidar_gps_entries.push_back("gps"); - - std::vector camera_entries; - camera_entries.push_back("camera"); - - - EntryManager em(required_entries,lidar_gps_entries,camera_entries); - - EXPECT_EQ(0, em.is_lidar_gps_entry_required("lidar1")); - EXPECT_EQ(1, em.is_lidar_gps_entry_required("lidar2")); - EXPECT_EQ(2, em.is_lidar_gps_entry_required("gps")); - EXPECT_EQ(0, em.is_camera_entry_required("camera")); - } - - TEST(EntryManagerTest, testCarLidarGpsEntryCheck) - { - std::vector required_entries; - required_entries.push_back("ssc"); - - std::vector lidar_gps_entries; - lidar_gps_entries.push_back("lidar"); - lidar_gps_entries.push_back("gps"); - - std::vector camera_entries; - camera_entries.push_back("camera"); - - EntryManager em(required_entries,lidar_gps_entries,camera_entries); - - EXPECT_EQ(0, em.is_lidar_gps_entry_required("lidar")); - EXPECT_EQ(1, em.is_lidar_gps_entry_required("gps")); - EXPECT_EQ(0, em.is_camera_entry_required("camera")); - } - -} +} \ No newline at end of file diff --git a/yield_plugin/config/parameters.yaml b/yield_plugin/config/parameters.yaml index c7531c28cb..ca5b321613 100644 --- a/yield_plugin/config/parameters.yaml +++ b/yield_plugin/config/parameters.yaml @@ -3,10 +3,11 @@ # Value type: Desired # No unit acceleration_adjustment_factor: 4.0 -# Time horizon for collision detection +# Time horizon for collision detection for vehicles on the route +# NOTE: not applied for pedestrians or other non-vehicle obstacles on the road # Units: s # Value type: Desired -collision_horizon: 10.0 +on_route_vehicle_collision_horizon: 10.0 # Minimum speed for moving obstacle # Units: m/s # Value type: Desired @@ -20,9 +21,9 @@ tpmin: 2.0 # Value type: Desired yield_max_deceleration: 3.0 # Minimum safety gap with an Object/Obstacle -# Units: s +# Units: meters # Value type: Desired -x_gap: 2.0 +x_gap: 10.0 # Host vehicle length # Units: m # Value type: Measured @@ -52,6 +53,10 @@ acceptable_passed_timesteps: 5 # Unit: m # Value type: Desired intervehicle_collision_distance: 6.0 +# Radius to check for potential collision +# Unit: m +# Value type: Desired +collision_check_radius: 150.0 # Time gap to finish planning a yield earlier than collision time # Unit: s # Value type: Desired diff --git a/yield_plugin/include/yield_plugin/yield_config.hpp b/yield_plugin/include/yield_plugin/yield_config.hpp index 4aaec0090e..0a81a83652 100644 --- a/yield_plugin/include/yield_plugin/yield_config.hpp +++ b/yield_plugin/include/yield_plugin/yield_config.hpp @@ -24,7 +24,7 @@ struct YieldPluginConfig { double acceleration_adjustment_factor = 4.0; // Adjustment factor for safe and comfortable acceleration/deceleration - double collision_horizon = 10.0; // time horizon for collision detection in s + double on_route_vehicle_collision_horizon = 10.0; // time horizon for collision detection in s double min_obstacle_speed = 2.0; // Minimum speed for moving obstacle in m/s double tpmin = 2.0; // minimum object avoidance planning time in s double yield_max_deceleration = 3.0; // max deceleration value in m/s^2 @@ -42,12 +42,13 @@ struct YieldPluginConfig bool enable_adjustable_gap = true; // Flag to enable yield plugin to check for adjustable gap for example digital gap from map int acceptable_urgency = 5; //Minimum urgency value to consider the mobility request double speed_moving_average_window_size = 3.0; //Window size for speed moving average filter + double collision_check_radius = 150.0; //Radius to check for potential collision friend std::ostream& operator<<(std::ostream& output, const YieldPluginConfig& c) { output << "YieldPluginConfig { " << std::endl << "acceleration_adjustment_factor: " << c.acceleration_adjustment_factor << std::endl - << "collision_horizon: " << c.collision_horizon << std::endl + << "on_route_vehicle_collision_horizon: " << c.on_route_vehicle_collision_horizon << std::endl << "min_obstacle_speed: " << c.min_obstacle_speed << std::endl << "yield_max_deceleration: " << c.yield_max_deceleration << std::endl << "x_gap: " << c.x_gap << std::endl @@ -64,6 +65,7 @@ struct YieldPluginConfig << "enable_adjustable_gap: " << c.enable_adjustable_gap << std::endl << "acceptable_urgency: " << c.acceptable_urgency << std::endl << "speed_moving_average_window_size: " << c.speed_moving_average_window_size << std::endl + << "collision_check_radius: " << c.collision_check_radius << std::endl << "}" << std::endl; return output; } diff --git a/yield_plugin/include/yield_plugin/yield_plugin.hpp b/yield_plugin/include/yield_plugin/yield_plugin.hpp index 5f27e51b5c..9979a219f8 100644 --- a/yield_plugin/include/yield_plugin/yield_plugin.hpp +++ b/yield_plugin/include/yield_plugin/yield_plugin.hpp @@ -37,8 +37,6 @@ #include #include #include -#include -#include #include #include #include @@ -96,9 +94,10 @@ class YieldPlugin * \brief trajectory is modified to safely avoid obstacles on the road * \param original_tp original trajectory plan without object avoidance * \param current_speed_ current speed of the vehicle + * \param * \return modified trajectory plan */ - carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_velocity); + carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity); /** * \brief calculate quintic polynomial equation for a given x @@ -208,7 +207,7 @@ class YieldPlugin /** * \brief Looks up the transform between map and earth frames, and sets the member variable */ - void lookupECEFtoMapTransform(); + void lookup_ecef_to_map_transform(); /** * \brief checks trajectory for minimum gap associated with it @@ -218,11 +217,25 @@ class YieldPlugin double check_traj_for_digital_min_gap(const carma_planning_msgs::msg::TrajectoryPlan& original_tp) const; /** - * \brief Callback for map projection string to define lat/lon -> map conversion - * \brief msg The proj string defining the projection. + * \brief Setter for map projection string to define lat/lon -> map conversion + * \param georeference The proj string defining the projection. */ - void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg); + void set_georeference_string(const std::string& georeference); + /** + * \brief Setter for external objects with predictions in the environment + * \param object_list The object list. + */ + void set_external_objects(const std::vector& object_list); + + /** + * \brief Return collision time given two trajectories + * \param trajectory1 trajectory of the ego vehicle + * \param trajectory2 trajectory of the obstacle + * \param collision_radius a distance to check between two trajectory points at a same timestamp that is considered a collision + * \return time_of_collision if collision detected, otherwise, std::nullopt + */ + std::optional detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius); private: @@ -231,6 +244,8 @@ class YieldPlugin MobilityResponseCB mobility_response_publisher_; LaneChangeStatusCB lc_status_publisher_; std::shared_ptr nh_; + std::set route_llt_ids_; + std::vector external_objects_; // flag to show if it is possible for the vehicle to accept the cooperative request bool cooperative_request_acceptable_ = false; @@ -263,44 +278,5 @@ class YieldPlugin return res; } - // TODO replace with Basic Autonomy moving average filter - std::vector moving_average_filter(const std::vector input, int window_size, bool ignore_first_point=true) - { - if (window_size % 2 == 0) { - throw std::invalid_argument("moving_average_filter window size must be odd"); - } - - std::vector output; - output.reserve(input.size()); - - if (input.size() == 0) { - return output; - } - - int start_index = 0; - if (ignore_first_point) { - start_index = 1; - output.push_back(input[0]); - } - - for (int i = start_index; i sample; - sample.reserve(count); - for (int j = sample_min; j <= sample_max; j++) { - total += input[j]; - } - output.push_back(total / (double) count); - - } - - return output; - } - }; }; // namespace yield_plugin diff --git a/yield_plugin/include/yield_plugin/yield_plugin_node.hpp b/yield_plugin/include/yield_plugin/yield_plugin_node.hpp index 2881870932..161e521840 100644 --- a/yield_plugin/include/yield_plugin/yield_plugin_node.hpp +++ b/yield_plugin/include/yield_plugin/yield_plugin_node.hpp @@ -25,7 +25,7 @@ #include #include #include - +#include #include "yield_plugin.hpp" #include "yield_config.hpp" @@ -68,14 +68,14 @@ class YieldPluginNode : public carma_guidance_plugins::TacticalPlugin std::string version_id_; // Publishers - carma_ros2_utils::PubPtr mob_resp_pub_; - carma_ros2_utils::PubPtr lc_status_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr mob_resp_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr lc_status_pub_; // Subscribers - carma_ros2_utils::SubPtr mob_request_sub_; - carma_ros2_utils::SubPtr bsm_sub_; - carma_ros2_utils::SubPtr georeference_sub_; - + rclcpp::Subscription::SharedPtr mob_request_sub_; + rclcpp::Subscription::SharedPtr bsm_sub_; + rclcpp::Subscription::SharedPtr external_objects_sub_; + rclcpp::Subscription::SharedPtr georeference_sub_; }; } // namespace yield_plugin diff --git a/yield_plugin/package.xml b/yield_plugin/package.xml index 4b392d3340..d4b5d4ad8b 100644 --- a/yield_plugin/package.xml +++ b/yield_plugin/package.xml @@ -41,6 +41,7 @@ carma_perception_msgs carma_guidance_plugins tf2 + basic_autonomy ament_lint_auto ament_cmake_gtest diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index 80898a9c7f..e767af3b5e 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -30,11 +31,10 @@ #include #include #include -#include -#include #include #include #include +#include using oss = std::ostringstream; @@ -59,11 +59,11 @@ namespace yield_plugin // distance to consider trajectories colliding (chosen based on lane width and vehicle size) for (size_t i=0; iget_logger(),"Cannot process mobility request as map projection is not yet set!"); + RCLCPP_ERROR(nh_->get_logger(),"Cannot process mobility request as map projection is not yet set!"); return; } if (incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT || incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_RIGHT) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Cooperative Lane Change Request Received"); + RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received"); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED; lc_status_msg.description = "Received lane merge request"; if (incoming_request.m_header.recipient_id == config_.vehicle_id) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"CLC Request correctly received"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received"); } // extract mobility header std::string req_sender_id = incoming_request.m_header.sender_id; @@ -191,19 +191,19 @@ namespace yield_plugin lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED; lc_status_msg.description = "Accepted lane merge request"; response_to_clc_req = true; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"CLC accepted"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); } else { lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED; lc_status_msg.description = "Rejected lane merge request"; response_to_clc_req = false; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"CLC rejected"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); } carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req); mobility_response_publisher_(outgoing_response); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"response sent"); + RCLCPP_DEBUG(nh_->get_logger(),"response sent"); } } lc_status_publisher_(lc_status_msg); @@ -232,29 +232,32 @@ namespace yield_plugin if (req->initial_trajectory_plan.trajectory_points.size() < 2){ throw std::invalid_argument("Empty Trajectory received by Yield"); } + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + rclcpp::Time start_time = system_clock.now(); // Start timing the execution time for planning so it can be logged + carma_planning_msgs::msg::TrajectoryPlan original_trajectory = req->initial_trajectory_plan; carma_planning_msgs::msg::TrajectoryPlan yield_trajectory; // seperating cooperative yield with regular object detection for better performance. if (config_.enable_cooperative_behavior && clc_urgency_ > config_.acceptable_urgency) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Only consider high urgency clc"); + RCLCPP_DEBUG(nh_->get_logger(),"Only consider high urgency clc"); if (timesteps_since_last_req_ < config_.acceptable_passed_timesteps) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep"); + RCLCPP_DEBUG(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep"); yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, req->vehicle_state.longitudinal_vel); timesteps_since_last_req_++; } else { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); - yield_trajectory = update_traj_for_object(original_trajectory, req->vehicle_state.longitudinal_vel); // Compute the trajectory + RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); + yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory } } else { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Yield for object avoidance"); - yield_trajectory = update_traj_for_object(original_trajectory, req->vehicle_state.longitudinal_vel); // Compute the trajectory + RCLCPP_DEBUG(nh_->get_logger(),"Yield for object avoidance"); + yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory } yield_trajectory.header.frame_id = "map"; yield_trajectory.header.stamp = nh_->now(); @@ -262,7 +265,12 @@ namespace yield_plugin yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. resp->trajectory_plan = yield_trajectory; - + + rclcpp::Time end_time = system_clock.now(); // Planning complete + + auto duration = end_time - start_time; + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds())); + } carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_cooperative_behavior(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double current_speed) @@ -279,8 +287,8 @@ namespace yield_plugin for (size_t i=0; iget_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); + RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); cooperative_trajectory = original_tp; } @@ -328,7 +336,7 @@ namespace yield_plugin else { cooperative_request_acceptable_ = true; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory"); + RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory"); cooperative_trajectory = original_tp; } @@ -372,27 +380,32 @@ namespace yield_plugin calculated_speeds.push_back(dv); } // moving average filter - std::vector filtered_speeds = moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size); + std::vector filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size); double prev_speed = filtered_speeds[0]; for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ ) { carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp; - double current_speed = filtered_speeds[i]; + double current_speed = filtered_speeds.at(i); double traj_target_time = i * planning_time / original_tp.trajectory_points.size(); if (current_speed >= config_.max_stop_speed) { - double dt = (2 * original_traj_downtracks[i]) / (current_speed + prev_speed); - jmt_tpp = original_tp.trajectory_points[i]; - jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points[i-1].target_time) + rclcpp::Duration(dt*1e9); + double dt = (2 * original_traj_downtracks.at(i)) / (current_speed + prev_speed); + jmt_tpp = original_tp.trajectory_points.at(i); + jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.at(i-1).target_time) + rclcpp::Duration(dt*1e9); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "non-empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); + jmt_trajectory_points.push_back(jmt_tpp); } else { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"target speed is zero"); + RCLCPP_DEBUG(nh_->get_logger(),"target speed is zero"); // if speed is zero, the vehicle will stay in previous location. - jmt_tpp = jmt_trajectory_points[i-1]; - jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9); + jmt_tpp = jmt_trajectory_points.at(i-1); + // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase + jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points[i -1 ].target_time).seconds()) * 1e9); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); + jmt_trajectory_points.push_back(jmt_tpp); } prev_speed = current_speed; @@ -404,78 +417,211 @@ namespace yield_plugin return jmt_trajectory; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_velocity) + std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) { - + // Iterate through each pair of consecutive points in the trajectories + + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: " << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size()); + + // Iterate through the object to check if it's on the route + bool on_route = false; + int on_route_idx = 0; + + for (size_t j = 0; j < trajectory2.size(); ++j) + { + lanelet::BasicPoint2d curr_point; + curr_point.x() = trajectory2.at(j).predicted_position.position.x; + curr_point.y() = trajectory2.at(j).predicted_position.position.y; + + auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 4); // get 4 lanelets + + for (const auto& llt: corresponding_lanelets) + { + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id()); + + if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end()) + { + on_route = true; + on_route_idx = j; + break; + } + } + if (on_route) + break; + } + + if (!on_route) + { + RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring"); + return std::nullopt; + } + + double smallest_dist = std::numeric_limits::max(); + for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) + { + auto p1a = trajectory1.trajectory_points.at(i); + auto p1b = trajectory1.trajectory_points.at(i + 1); + for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) + { + auto p2a = trajectory2.at(j); + auto p2b = trajectory2.at(j + 1); + double p1a_t = rclcpp::Time(p1a.target_time).seconds(); + double p1b_t = rclcpp::Time(p1b.target_time).seconds(); + double p2a_t = rclcpp::Time(p2a.header.stamp).seconds(); + double p2b_t = rclcpp::Time(p2b.header.stamp).seconds(); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t)); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t)); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y); + + // Linearly interpolate positions at a common timestamp for both trajectories + double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t); + double x1 = p1a.x + dt * (p1b.x - p1a.x); + double y1 = p1a.y + dt * (p1b.y - p1a.y); + double x2 = p2a.predicted_position.position.x; + double y2 = p2a.predicted_position.position.y; + + // Calculate the distance between the two interpolated points + double distance = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); + smallest_dist = std::min(distance, smallest_dist); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt << ", x1: " << x1 << ", y1: " < config_.collision_check_radius) + { + RCLCPP_DEBUG(nh_->get_logger(), "Too far away" ); + return std::nullopt; + } + + if (distance > collision_radius) + { + continue; + } + + // if within collision radius + lanelet::BasicPoint2d vehicle_point(x1,y1); + lanelet::BasicPoint2d object_point(x2,y2); + double vehicle_downtrack = wm_->routeTrackPos(vehicle_point).downtrack; + double object_downtrack = wm_->routeTrackPos(object_point).downtrack; + + if (vehicle_downtrack > object_downtrack + config_.vehicle_length / 2) // if half a length of the vehicle past, it is considered behind + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Detected an object nearby behind the vehicle at timestamp " << std::to_string(p2a_t)); + return std::nullopt; + } + else + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 << ", within distance: " << distance); + return rclcpp::Time(p2a.header.stamp); + } + } + } + + // No collision detected + return std::nullopt; + } + + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) + { + if (original_tp.trajectory_points.empty()) + { + RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received empty trajectory plan in update_traj_for_object"); + throw std::invalid_argument("Yield plugin received empty trajectory plan in update_traj_for_object"); + } + + rclcpp::Time plan_start_time = original_tp.trajectory_points[0].target_time; carma_planning_msgs::msg::TrajectoryPlan update_tpp_vector; geometry_msgs::msg::Twist current_velocity; current_velocity.linear.x = initial_velocity; - std::vector rwol = wm_->getRoadwayObjects(); - carma_perception_msgs::msg::RoadwayObstacleList rwol2; - rwol2.roadway_obstacles = rwol; - host_vehicle_size.x = config_.vehicle_length; - host_vehicle_size.y = config_.vehicle_width; - host_vehicle_size.z = config_.vehicle_height; + std::optional earliest_collision_obj = std::nullopt; + std::set checked_external_object_ids; + std::vector new_list; + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size()); - std::vector rwol_collision; + if (wm_->getRoute() == nullptr) + { + RCLCPP_WARN(nh_->get_logger(), "No route available!"); + return original_tp; //route not available + } - // std::vector rwol_collision = carma_wm::collision_detection::WorldCollisionDetection(rwol2, original_tp, host_vehicle_size, current_velocity, config_.collision_horizon); - - lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y); - double vehicle_downtrack = wm_->routeTrackPos(point).downtrack; - - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack"); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),vehicle_downtrack); + // save route Ids for faster access + for (const auto& llt: wm_->getRoute()->shortestPath()) + { + route_llt_ids_.insert(llt.id()); + } - for (auto i : rwol2.roadway_obstacles){ + double earliest_collision_obj_time = std::numeric_limits::max(); - lanelet::BasicPoint2d point_o(i.object.pose.pose.position.x, i.object.pose.pose.position.y); - double object_down_track = wm_->routeTrackPos(point_o).downtrack; + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size()); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_down_track"); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),object_down_track); - - double dist_to_object = object_down_track - vehicle_downtrack; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_down_track - vehicle_downtrack"); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),dist_to_object); - - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"i.object.velocity.twist.linear.x"); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),i.object.velocity.twist.linear.x); - - if(current_velocity.linear.x > 0.0) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"(object_down_downtrack - vehicle_downtrack)/current_velocity.linear.x"); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),dist_to_object/current_velocity.linear.x); - - // Check to see if the object is in front of us and has a time-to-collision lower than our - // desired horizon - if(dist_to_object >= 0 && - dist_to_object / current_velocity.linear.x < config_.collision_horizon) { - rwol_collision.push_back(i); + for (const auto& curr_obstacle : external_objects) + { + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds()) << ", plan_start_time: " << std::to_string(plan_start_time.seconds())); + + // do not process outdated objects + if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp) > plan_start_time) + { + carma_perception_msgs::msg::PredictedState curr_state; + // artificially include current position as one of the predicted states + curr_state.header.stamp = curr_obstacle.header.stamp; + curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x; + curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y; + curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x; + new_list.push_back(curr_state); + new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend()); + + auto collision_time = detect_collision_time(original_tp, new_list, config_.intervehicle_collision_distance); + + if (collision_time != std::nullopt) + { + checked_external_object_ids.insert(curr_obstacle.id); + + // only save collision data that is the earliest + if (earliest_collision_obj_time >= collision_time.value().seconds()) + { + earliest_collision_obj = curr_obstacle; + earliest_collision_obj_time = collision_time.value().seconds(); } + } } - } - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Roadway Object List (rwol) size: " << rwol.size()); + lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y); + double vehicle_downtrack = wm_->routeTrackPos(point).downtrack; + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); - // correct the input types - if(!rwol_collision.empty()) + // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, + // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. + + if(earliest_collision_obj.has_value()) { RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!"); + lanelet::BasicPoint2d point_o(earliest_collision_obj.value().pose.pose.position.x, earliest_collision_obj.value().pose.pose.position.y); + double object_downtrack = wm_->routeTrackPos(point_o).downtrack; + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); + + double dist_to_object = object_downtrack - vehicle_downtrack; + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); + // Distance from the original trajectory point to the lead vehicle/object - double dist_x = rwol_collision[0].object.pose.pose.position.x - original_tp.trajectory_points[0].x; - double dist_y = rwol_collision[0].object.pose.pose.position.y - original_tp.trajectory_points[0].y; + double dist_x = earliest_collision_obj.value().pose.pose.position.x - original_tp.trajectory_points[0].x; + double dist_y = earliest_collision_obj.value().pose.pose.position.y - original_tp.trajectory_points[0].y; double x_lead = sqrt(dist_x*dist_x + dist_y*dist_y); // roadway object position double gap_time = (x_lead - config_.x_gap)/initial_velocity; - double collision_time = 0; //\TODO comming from carma_wm collision detection in future (CAR 4288) - - double goal_velocity = rwol_collision[0].object.velocity.twist.linear.x; + double goal_velocity = earliest_collision_obj.value().velocity.twist.linear.x; // determine the safety inter-vehicle gap based on speed double safety_gap = std::max(goal_velocity * gap_time, config_.x_gap); if (config_.enable_adjustable_gap) @@ -490,19 +636,18 @@ namespace yield_plugin double goal_pos = x_lead - safety_gap; if (goal_velocity <= config_.min_obstacle_speed){ - RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving"); + RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving"); } - double initial_time = 0; double initial_pos = 0.0; //relative initial position (first trajectory point) double initial_accel = 0; double goal_accel = 0; - double delta_v_max = fabs(rwol_collision[0].object.velocity.twist.linear.x - max_trajectory_speed(original_tp.trajectory_points)); + double delta_v_max = fabs(earliest_collision_obj.value().velocity.twist.linear.x - max_trajectory_speed(original_tp.trajectory_points)); // reference time, is the maximum time available to perform object avoidance (length of a trajectory) - double t_ref = (rclcpp::Time(original_tp.trajectory_points[original_tp.trajectory_points.size() - 1].target_time).seconds() - rclcpp::Time(original_tp.trajectory_points[0].target_time).seconds()); + double t_ref = (rclcpp::Time(original_tp.trajectory_points[original_tp.trajectory_points.size() - 1].target_time).seconds() - plan_start_time.seconds()); // time required for comfortable deceleration double t_ph = config_.acceleration_adjustment_factor * delta_v_max / config_.yield_max_deceleration; @@ -528,7 +673,8 @@ namespace yield_plugin return update_tpp_vector; } - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"No collision detection, so trajectory not modified."); + + RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified."); return original_tp; } @@ -537,12 +683,12 @@ namespace yield_plugin { std::vector downtracks; downtracks.reserve(trajectory_plan.trajectory_points.size()); - // relative downtrack distance of the fist point is 0.0 + // relative downtrack distance of the fist Point is 0.0 downtracks[0] = 0.0; for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){ - double dx = trajectory_plan.trajectory_points[i].x - trajectory_plan.trajectory_points[i-1].x; - double dy = trajectory_plan.trajectory_points[i].y - trajectory_plan.trajectory_points[i-1].y; - downtracks[i] = sqrt(dx*dx + dy*dy); + double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x; + double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y; + downtracks.at(i) = sqrt(dx*dx + dy*dy); } return downtracks; } @@ -552,7 +698,7 @@ namespace yield_plugin double result = 0; for (size_t i = 0; i < coeff.size(); i++) { - double value = coeff[i] * pow(x, (int)(coeff.size() - 1 - i)); + double value = coeff.at(i) * pow(x, (int)(coeff.size() - 1 - i)); result = result + value; } return result; @@ -563,7 +709,7 @@ namespace yield_plugin double result = 0; for (size_t i = 0; i < coeff.size()-1; i++) { - double value = (int)(coeff.size() - 1 - i) * coeff[i] * pow(x, (int)(coeff.size() - 2 - i)); + double value = (int)(coeff.size() - 1 - i) * coeff.at(i) * pow(x, (int)(coeff.size() - 2 - i)); result = result + value; } return result; @@ -574,10 +720,10 @@ namespace yield_plugin double max_speed = 0; for(size_t i = 0; i < trajectory_points.size() - 2; i++ ) { - double dx = trajectory_points[i + 1].x - trajectory_points[i].x; - double dy = trajectory_points[i + 1].y - trajectory_points[i].y; + double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x; + double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y; double d = sqrt(dx*dx + dy*dy); - double t = (rclcpp::Time(trajectory_points[i + 1].target_time).seconds() - rclcpp::Time(trajectory_points[i].target_time).seconds()); + double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds()); double v = d/t; if(v > max_speed) { @@ -593,13 +739,13 @@ namespace yield_plugin for (size_t i = 0; i < original_tp.trajectory_points.size(); i++) { - lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points[i].x, original_tp.trajectory_points[i].y); + lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points.at(i).x, original_tp.trajectory_points.at(i).y); auto llts = wm_->getLaneletsFromPoint(veh_pos, 1); if (llts.empty()) { - RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points[i].x << "y="<< original_tp.trajectory_points[i].y); + RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y); - throw std::invalid_argument("Trajectory point is not on a valid lanelet."); + throw std::invalid_argument("Trajectory Point is not on a valid lanelet."); } auto digital_min_gap = llts[0].regulatoryElementsAs(); //Returns a list of these elements) if (!digital_min_gap.empty()) @@ -612,9 +758,14 @@ namespace yield_plugin return desired_gap; } - void YieldPlugin::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg) + void YieldPlugin::set_georeference_string(const std::string& georeference) + { + map_projector_ = std::make_shared(georeference.c_str()); // Build projector from proj string + } + + void YieldPlugin::set_external_objects(const std::vector& object_list) { - map_projector_ = std::make_shared(msg->data.c_str()); // Build projector from proj string + external_objects_ = object_list; } } // namespace yield_plugin diff --git a/yield_plugin/src/yield_plugin_node.cpp b/yield_plugin/src/yield_plugin_node.cpp index 3f2d8b9a99..15525b69a6 100644 --- a/yield_plugin/src/yield_plugin_node.cpp +++ b/yield_plugin/src/yield_plugin_node.cpp @@ -28,7 +28,8 @@ namespace yield_plugin // Declare parameters config_.acceleration_adjustment_factor = declare_parameter("acceleration_adjustment_factor", config_.acceleration_adjustment_factor); config_.min_obstacle_speed = declare_parameter("min_obstacle_speed", config_.min_obstacle_speed); - config_.collision_horizon = declare_parameter("collision_horizon", config_.collision_horizon); + config_.on_route_vehicle_collision_horizon = declare_parameter("on_route_vehicle_collision_horizon", config_.on_route_vehicle_collision_horizon); + config_.collision_check_radius = declare_parameter("collision_check_radius", config_.collision_check_radius); config_.yield_max_deceleration = declare_parameter("yield_max_deceleration", config_.yield_max_deceleration); config_.tpmin = declare_parameter("tpmin", config_.tpmin); config_.x_gap = declare_parameter("x_gap", config_.x_gap); @@ -54,7 +55,8 @@ namespace yield_plugin get_parameter("acceleration_adjustment_factor", config_.acceleration_adjustment_factor); get_parameter("min_obstacle_speed", config_.min_obstacle_speed); - get_parameter("collision_horizon", config_.collision_horizon); + get_parameter("on_route_vehicle_collision_horizon", config_.on_route_vehicle_collision_horizon); + get_parameter("collision_check_radius", config_.collision_check_radius); get_parameter("yield_max_deceleration", config_.yield_max_deceleration); get_parameter("x_gap", config_.x_gap); get_parameter("max_stop_speed", config_.max_stop_speed); @@ -73,11 +75,12 @@ namespace yield_plugin get_parameter("vehicle_width", config_.vehicle_width); get_parameter("vehicle_id", config_.vehicle_id); - RCLCPP_INFO_STREAM(rclcpp::get_logger("yield_plugin"), "YieldPlugin Params" << config_); + RCLCPP_INFO_STREAM(get_logger(), "YieldPlugin Params: " << config_); worker_ = std::make_shared(shared_from_this(), get_world_model(), config_, [this](auto msg) { mob_resp_pub_->publish(msg); }, [this](auto msg) { lc_status_pub_->publish(msg); }); + // Publisher mob_resp_pub_ = create_publisher("outgoing_mobility_response", 1); lc_status_pub_ = create_publisher("cooperative_lane_change_status", 10); @@ -85,8 +88,16 @@ namespace yield_plugin // Subscriber mob_request_sub_ = create_subscription("incoming_mobility_request", 10, std::bind(&YieldPlugin::mobilityrequest_cb,worker_.get(),std_ph::_1)); bsm_sub_ = create_subscription("bsm_outbound", 1, std::bind(&YieldPlugin::bsm_cb,worker_.get(),std_ph::_1)); - georeference_sub_ = create_subscription("georeference", 10, std::bind(&YieldPlugin::georeferenceCallback,worker_.get(),std_ph::_1)); - + georeference_sub_ = create_subscription("georeference", 10, + [this](const std_msgs::msg::String::SharedPtr msg) { + worker_->set_georeference_string(msg->data); + }); + + // Return success if everything initialized successfully + external_objects_sub_ = create_subscription("external_object_predictions", 20, + [this](const carma_perception_msgs::msg::ExternalObjectList::SharedPtr msg) { + worker_->set_external_objects(msg->objects); + }); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } diff --git a/yield_plugin/test/test_yield.cpp b/yield_plugin/test/test_yield.cpp index 65336066d6..9f70303fa8 100755 --- a/yield_plugin/test/test_yield.cpp +++ b/yield_plugin/test/test_yield.cpp @@ -21,7 +21,6 @@ #include #include #include - #include #include #include @@ -39,8 +38,6 @@ #include #include #include -#include -#include #include using namespace yield_plugin; @@ -52,7 +49,7 @@ TEST(YieldPluginTest, test_polynomial_calc) std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); std::vector coeff; coeff.push_back(2.0); @@ -81,7 +78,7 @@ TEST(YieldPluginTest, test_polynomial_calc_derivative) std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); std::vector coeff; coeff.push_back(2.0); @@ -110,7 +107,7 @@ TEST(YieldPluginTest, MaxTrajectorySpeed) std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); std::vector trajectory_points; @@ -187,11 +184,9 @@ TEST(YieldPluginTest, test_update_traj) // std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); - - + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); - carma_perception_msgs::msg::RoadwayObstacleList rwol; + carma_perception_msgs::msg::ExternalObjectList rwol; carma_planning_msgs::msg::TrajectoryPlan tp; rclcpp::Time startTime(1.0); @@ -204,8 +199,8 @@ TEST(YieldPluginTest, test_update_traj) carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_6; carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_7; - trajectory_point_1.x = 1.0; - trajectory_point_1.y = 1.0; + trajectory_point_1.x = 10.0; + trajectory_point_1.y = 0.0001; trajectory_point_1.target_time = rclcpp::Time(0); trajectory_point_2.x = 10.0; @@ -234,26 +229,26 @@ TEST(YieldPluginTest, test_update_traj) tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4, trajectory_point_5, trajectory_point_6, trajectory_point_7}; - carma_perception_msgs::msg::RoadwayObstacle rwo_1; + carma_perception_msgs::msg::ExternalObject rwo_1; tf2::Quaternion tf_orientation; tf_orientation.setRPY(0, 0, 1.5708); - rwo_1.object.pose.pose.position.x = 60; - rwo_1.object.pose.pose.position.y = 50; - rwo_1.object.pose.pose.position.z = 0; + rwo_1.pose.pose.position.x = 60; + rwo_1.pose.pose.position.y = 50; + rwo_1.pose.pose.position.z = 0; - rwo_1.object.pose.pose.orientation.x = tf_orientation.getX(); - rwo_1.object.pose.pose.orientation.y = tf_orientation.getY(); - rwo_1.object.pose.pose.orientation.z = tf_orientation.getZ(); - rwo_1.object.pose.pose.orientation.w = tf_orientation.getW(); + rwo_1.pose.pose.orientation.x = tf_orientation.getX(); + rwo_1.pose.pose.orientation.y = tf_orientation.getY(); + rwo_1.pose.pose.orientation.z = tf_orientation.getZ(); + rwo_1.pose.pose.orientation.w = tf_orientation.getW(); - rwo_1.object.size.x = 1; - rwo_1.object.size.y = 1; - rwo_1.object.size.z = 1; + rwo_1.size.x = 1; + rwo_1.size.y = 1; + rwo_1.size.z = 1; carma_perception_msgs::msg::PredictedState ps_1; - ps_1.header.stamp.nanosec = 1000; + ps_1.header.stamp.sec = 1; ps_1.predicted_position.position.x = 10; ps_1.predicted_position.position.y = 10; @@ -265,7 +260,7 @@ TEST(YieldPluginTest, test_update_traj) ps_1.predicted_position.orientation.w = tf_orientation.getW(); carma_perception_msgs::msg::PredictedState ps_2; - ps_2.header.stamp.nanosec = 2000; + ps_2.header.stamp.sec = 2; ps_2.predicted_position.position.x = 10; ps_2.predicted_position.position.y = 20; @@ -277,7 +272,7 @@ TEST(YieldPluginTest, test_update_traj) ps_2.predicted_position.orientation.w = tf_orientation.getW(); carma_perception_msgs::msg::PredictedState ps_3; - ps_3.header.stamp.nanosec = 3000; + ps_3.header.stamp.sec = 3; ps_3.predicted_position.position.x = 10; ps_3.predicted_position.position.y = 30; @@ -288,26 +283,228 @@ TEST(YieldPluginTest, test_update_traj) ps_3.predicted_position.orientation.z = tf_orientation.getZ(); ps_3.predicted_position.orientation.w = tf_orientation.getW(); - rwo_1.object.predictions = {ps_1,ps_2,ps_3}; - rwo_1.object.velocity.twist.linear.x = 5.0; + rwo_1.predictions = {ps_1,ps_2,ps_3}; + rwo_1.velocity.twist.linear.x = 10.0; - rwol.roadway_obstacles = {rwo_1}; + rwol.objects = {rwo_1}; - - std::vector rw_objs; + std::vector rw_objs; rw_objs.push_back(rwo_1); - wm->setRoadwayObjects(rw_objs); + carma_planning_msgs::msg::TrajectoryPlan tp_new = plugin.update_traj_for_object(tp, rw_objs, 10.0); - carma_planning_msgs::msg::TrajectoryPlan tp_new = plugin.update_traj_for_object(tp, 10.0); + EXPECT_EQ(7, tp.trajectory_points.size()); +} - for (size_t i = 1; i < tp_new.trajectory_points.size(); i++) { - std::cout << tp_new.trajectory_points[i].x<< tp_new.trajectory_points[i].y << std::endl; - } +TEST(YieldPluginTest, detect_collision_time) +{ + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(100,100); + + wm->setMap(map); + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + YieldPluginConfig config; + config.vehicle_length = 4; + config.vehicle_width = 2; + config.vehicle_height = 1; + config.collision_check_radius = 90; + + // std::shared_ptr wm = std::make_shared(); + auto nh = std::make_shared(rclcpp::NodeOptions()); - EXPECT_EQ(7, tp.trajectory_points.size()); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); + + carma_perception_msgs::msg::ExternalObjectList rwol; + carma_planning_msgs::msg::TrajectoryPlan tp; + + EXPECT_THROW(plugin.update_traj_for_object(tp, {}, 0.0), std::invalid_argument); + + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_1; + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_2; + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_3; + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_4; + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_5; + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_6; + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point_7; + + trajectory_point_1.x = 10.0; + trajectory_point_1.y = 0.0001; + trajectory_point_1.target_time = rclcpp::Time(0); + + trajectory_point_2.x = 10.0; + trajectory_point_2.y = 20.0; + trajectory_point_2.target_time = rclcpp::Time(1,0); + + trajectory_point_3.x = 10.0; + trajectory_point_3.y = 30.0; + trajectory_point_3.target_time = rclcpp::Time(2,0); + + trajectory_point_4.x = 10.0; + trajectory_point_4.y = 40.0; + trajectory_point_4.target_time = rclcpp::Time(3,0); + + trajectory_point_5.x = 10.0; + trajectory_point_5.y = 50.0; + trajectory_point_5.target_time = rclcpp::Time(4,0); + + trajectory_point_6.x = 10.0; + trajectory_point_6.y = 60.0; + trajectory_point_6.target_time = rclcpp::Time(5,0); + + trajectory_point_7.x = 10.0; + trajectory_point_7.y = 70.0; + trajectory_point_7.target_time = rclcpp::Time(6,0); + + tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4, trajectory_point_5, trajectory_point_6, trajectory_point_7}; + + carma_perception_msgs::msg::ExternalObject rwo_1; + + // Set route but also test no throw + EXPECT_NO_THROW(plugin.update_traj_for_object(tp, {}, 0.0)); + + // ON ROUTE, BUT NO COLLISION DUE TO BEING AHEAD + + tf2::Quaternion tf_orientation; + tf_orientation.setRPY(0, 0, 1.5708); + + rwo_1.pose.pose.position.x = 60; + rwo_1.pose.pose.position.y = 50; + rwo_1.pose.pose.position.z = 0; + + rwo_1.pose.pose.orientation.x = tf_orientation.getX(); + rwo_1.pose.pose.orientation.y = tf_orientation.getY(); + rwo_1.pose.pose.orientation.z = tf_orientation.getZ(); + rwo_1.pose.pose.orientation.w = tf_orientation.getW(); + + rwo_1.size.x = 1; + rwo_1.size.y = 1; + rwo_1.size.z = 1; + + carma_perception_msgs::msg::PredictedState ps_1; + ps_1.header.stamp.sec = 1; + + ps_1.predicted_position.position.x = 10; + ps_1.predicted_position.position.y = 10; + ps_1.predicted_position.position.z = 0; + ps_1.predicted_position.orientation.x = tf_orientation.getX(); + ps_1.predicted_position.orientation.y = tf_orientation.getY(); + ps_1.predicted_position.orientation.z = tf_orientation.getZ(); + ps_1.predicted_position.orientation.w = tf_orientation.getW(); + + carma_perception_msgs::msg::PredictedState ps_2; + ps_2.header.stamp.sec = 2; + + ps_2.predicted_position.position.x = 10; + ps_2.predicted_position.position.y = 20; + ps_2.predicted_position.position.z = 0; + + ps_2.predicted_position.orientation.x = tf_orientation.getX(); + ps_2.predicted_position.orientation.y = tf_orientation.getY(); + ps_2.predicted_position.orientation.z = tf_orientation.getZ(); + ps_2.predicted_position.orientation.w = tf_orientation.getW(); + + carma_perception_msgs::msg::PredictedState ps_3; + ps_3.header.stamp.sec = 3; + + ps_3.predicted_position.position.x = 10; + ps_3.predicted_position.position.y = 30; + ps_3.predicted_position.position.z = 0; + + ps_3.predicted_position.orientation.x = tf_orientation.getX(); + ps_3.predicted_position.orientation.y = tf_orientation.getY(); + ps_3.predicted_position.orientation.z = tf_orientation.getZ(); + ps_3.predicted_position.orientation.w = tf_orientation.getW(); + + rwo_1.predictions = {ps_1,ps_2,ps_3}; + rwo_1.velocity.twist.linear.x = 10.0; + + std::optional collision_time = plugin.detect_collision_time(tp, rwo_1.predictions, 6); + + ASSERT_TRUE(collision_time == std::nullopt); + + // DETECT COLLISION + + ps_1.header.stamp.sec = 1; + + ps_1.predicted_position.position.x = 10; + ps_1.predicted_position.position.y = 30; + ps_1.predicted_position.position.z = 0; + + ps_2.header.stamp.sec = 2; + + ps_2.predicted_position.position.x = 10; + ps_2.predicted_position.position.y = 31; + ps_2.predicted_position.position.z = 0; + + ps_3.header.stamp.sec = 3; + + ps_3.predicted_position.position.x = 10; + ps_3.predicted_position.position.y = 32; + ps_3.predicted_position.position.z = 0; + + rwo_1.predictions = {ps_1,ps_2,ps_3}; + // + + collision_time = plugin.detect_collision_time(tp, rwo_1.predictions, 6); + ASSERT_TRUE(collision_time != std::nullopt); + ASSERT_TRUE(collision_time.value() == rclcpp::Time(2, 0, collision_time.value().get_clock_type())); + + // STATES ARE NOT ON ROUTE + ps_1.header.stamp.sec = 1; + + ps_1.predicted_position.position.x = 20; + ps_1.predicted_position.position.y = 30; + ps_1.predicted_position.position.z = 0; + + ps_2.header.stamp.sec = 2; + + ps_2.predicted_position.position.x = 20; + ps_2.predicted_position.position.y = 31; + ps_2.predicted_position.position.z = 0; + + rwo_1.predictions = {ps_1,ps_2}; + + collision_time = plugin.detect_collision_time(tp, rwo_1.predictions, 6); + ASSERT_TRUE(collision_time == std::nullopt); + + // STATES ARE ON THE ROUTE, BUT TOO FAR AWAY + ps_1.header.stamp.sec = 1; + + ps_1.predicted_position.position.x = 10; + ps_1.predicted_position.position.y = 95; + ps_1.predicted_position.position.z = 0; + + ps_2.header.stamp.sec = 2; + + ps_2.predicted_position.position.x = 10; + ps_2.predicted_position.position.y = 96; + ps_2.predicted_position.position.z = 0; + + rwo_1.predictions = {ps_1,ps_2}; + + collision_time = plugin.detect_collision_time(tp, rwo_1.predictions, 6); + ASSERT_TRUE(collision_time == std::nullopt); + + // STATES ARE ON THE ROUTE, BUT ALREADY PASSED + ps_1.header.stamp.sec = 2; + + ps_1.predicted_position.position.x = 10; + ps_1.predicted_position.position.y = 25; + ps_1.predicted_position.position.z = 0; + + ps_2.header.stamp.sec = 3; + + ps_2.predicted_position.position.x = 10; + ps_2.predicted_position.position.y = 35; + ps_2.predicted_position.position.z = 0; + + rwo_1.predictions = {ps_1,ps_2}; + + collision_time = plugin.detect_collision_time(tp, rwo_1.predictions, 6); + ASSERT_TRUE(collision_time == std::nullopt); } TEST(YieldPluginTest, test_update_traj2) @@ -316,7 +513,7 @@ TEST(YieldPluginTest, test_update_traj2) std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); carma_planning_msgs::msg::TrajectoryPlan original_tp; @@ -418,7 +615,7 @@ TEST(YieldPluginTest, test_update_traj_stop) std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); carma_planning_msgs::msg::TrajectoryPlan original_tp; @@ -497,7 +694,7 @@ TEST(YieldPluginTest, test_update_traj_stop) if (dv >= 1.0) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"target speed is positive"); + RCLCPP_WARN(rclcpp::get_logger("yield_plugin"),"target speed is positive"); if (dv >= current_speed_){ dv = current_speed_; } @@ -508,7 +705,7 @@ TEST(YieldPluginTest, test_update_traj_stop) } else { - RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"target speed is zero"); + RCLCPP_WARN(rclcpp::get_logger("yield_plugin"),"target speed is zero"); new_tpp = new_trajectory_points[i-1]; new_tpp.target_time = rclcpp::Time(new_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9); new_trajectory_points.push_back(new_tpp); @@ -530,7 +727,7 @@ TEST(YieldPluginTest, jmt_traj) std::shared_ptr wm = std::make_shared(); auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); carma_planning_msgs::msg::TrajectoryPlan original_tp; @@ -609,7 +806,7 @@ TEST(YieldPluginTest, min_digital_gap) YieldPluginConfig config; auto nh = std::make_shared(rclcpp::NodeOptions()); - YieldPlugin plugin(nh,wm, config,[&](auto msg) {}, [&](auto msg) {}); + YieldPlugin plugin(nh,wm, config,[](const auto& msg) {}, [](const auto& msg) {}); carma_planning_msgs::msg::TrajectoryPlan original_tp; @@ -644,8 +841,3 @@ TEST(YieldPluginTest, min_digital_gap) EXPECT_EQ(gap, min_gap); } - - - - - diff --git a/yield_plugin/test/test_yield_plugin.cpp b/yield_plugin/test/test_yield_plugin.cpp index 7ec690e369..00c15febde 100755 --- a/yield_plugin/test/test_yield_plugin.cpp +++ b/yield_plugin/test/test_yield_plugin.cpp @@ -36,12 +36,12 @@ class YieldNode : public carma_ros2_utils::CarmaLifecycleNode void callback(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("yield_plugin"),"Test mob_resp callback.."); + RCLCPP_INFO(get_logger(),"Test mob_resp callback.."); } void status_callback(const carma_planning_msgs::msg::LaneChangeStatus::UniquePtr msg) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("yield_plugin"),"Test lc callback.."); + RCLCPP_INFO(get_logger(),"Test lc callback.."); } };